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Abstract 


This  thesis  reports  on  the  design  and  validation  of  estimation  and  planning  algorithms  for  un¬ 
derwater  vehicle  cooperative  localization.  While  attitude  and  depth  are  easily  instrumented  with 
bounded-error,  autonomous  underwater  vehicles  (AUVs)  have  no  internal  sensor  that  directly  ob¬ 
serves  XY  position.  The  global  positioning  system  (GPS)  and  other  radio-based  navigation  tech¬ 
niques  are  not  available  because  of  the  strong  attenuation  of  electromagnetic  signals  in  seawater. 
The  navigation  algorithms  presented  herein  fuse  local  body-frame  rate  and  attitude  measurements 
with  range  observations  between  vehicles  within  a  decentralized  architecture. 

The  acoustic  communication  channel  is  both  unreliable  and  low  bandwidth,  precluding  many 
state-of-the-art  terrestrial  cooperative  navigation  algorithms.  We  exploit  the  underlying  structure 
of  a  post-process  centralized  estimator  in  order  to  derive  two  real-time  decentralized  estimation 
frameworks.  First,  the  origin  state  method  enables  a  client  vehicle  to  exactly  reproduce  the  cor¬ 
responding  centralized  estimate  within  a  server-to-client  vehicle  network.  Second,  a  graph-based 
navigation  framework  produces  an  approximate  reconstruction  of  the  centralized  estimate  onboard 
each  vehicle.  Finally,  we  present  a  method  to  plan  a  locally  optimal  server  path  to  localize  a  client 
vehicle  along  a  desired  nominal  trajectory.  The  planning  algorithm  introduces  a  probabilistic  chan¬ 
nel  model  into  prior  Gaussian  belief  space  planning  frameworks. 

In  summary,  cooperative  localization  reduces  XY  position  error  growth  within  underwater  ve¬ 
hicle  networks.  Moreover,  these  methods  remove  the  reliance  on  static  beacon  networks,  which 
do  not  scale  to  large  vehicle  networks  and  limit  the  range  of  operations.  Each  proposed  localiza¬ 
tion  algorithm  was  validated  in  full-scale  AUV  field  trials.  The  planning  framework  was  evaluated 
through  numerical  simulation. 


Chapter  1 
Introduction 


1.1  Motivation 

Autonomous  underwater  vehicles  (AUVs)  have  equipped  oceanographers  with  the  ability  to  safely 
and  efficiently  explore  and  research  a  variety  of  difficult  environments  (Bellingham  and  Rajan, 
2007)  including  the  under-ice  Arctic  Ocean  (Jakuba  et  al.,  2008;  Kunz  et  al.,  2009),  the  deepest 
regions  of  the  Mariana  trench  (Bowen  et  al.,  2009;  Whitcomb  et  al.,  2010),  and  the  Great  Barrier 
Reef  (Williams  and  Mahon,  2004).  AUVs  require  robust  navigation  algorithms  to  safely  operate 
in  such  hostile  environments  and  geo-reference  the  valuable  scientific  data  that  they  collect. 

Water  is  opaque  to  electromagnetic  signals,  prohibiting  the  direct  use  of  the  global  positioning 
system  (GPS)  and  other  radio  frequency  based  navigation  techniques  while  submerged.  Terrain- 
aided  navigation  utilizes  prior  bathymetric  terrain  maps  for  localization,  but  is  not  applicable  to  op¬ 
erations  in  the  mid-depth  zone  where  rich  altitude  information  is  not  available.  Visually-aided  nav¬ 
igation,  commonly  used  in  terrestrial  and  aerial  robotics,  is  only  possible  when  an  AUV  is  close  to 
the  seafloor  due  to  backscatter,  strong  attenuation,  and  a  generally  unstructured  scene,  which  chal¬ 
lenge  state-of-the-art  computer  vision  algorithms.  Underwater  vehicles  typically  employ  acoustic 
beacon  networks,  such  as  narrowband  long-baseline  (LBL)  and  ultra- short-baseline  (USBL),  to 
obtain  accurate  bounded-error  navigation  in  the  mid-depth  zone  where  there  is  no  apparent  visual 
information.  Acoustic  beacon  methods,  however,  generally  require  additional  infrastructure  and 
limit  vehicle  operations  to  the  acoustic  footprint  of  the  beacons. 

Acoustic  modems  enable  vehicles  to  both  share  data  and  observe  their  relative  range.  How¬ 
ever,  the  underwater  acoustic  channel  is  unreliable,  exhibits  low-bandwidth,  and  suffers  from  high 
latency  (sound  is  orders  of  magnitude  slower  than  light).  Despite  these  challenges,  underwater 
navigation  can  benefit  from  cooperative  localization — each  vehicle  is  treated  as  a  mobile  acoustic 
navigation  beacon,  which  requires  no  additional  infrastructure  and  is  not  limited  in  the  range  of 
operations  by  static  beacons. 


1 


This  thesis  reports  on  novel  estimation  and  control  algorithms  for  synchronous -clock  coop¬ 
erative  underwater  localization.  The  contributions  of  this  work  include:  an  exact  one-to-many 
(server-to-client)  localization  algorithm,  an  approximate  many-to-many  (peer-to-peer)  cooperative 
localization  algorithm,  and  a  planning  framework  for  establishing  more  informative  geometries  for 
range-only  server-client  localization.  These  algorithms  are  designed  to  be  tolerant  to  the  extremely 
faulty  and  bandwidth-limited  acoustic  communication  channel.  Moreover,  they  are  not  limited  to 
AUVs  and  can  be  applied  to  any  network  of  platforms  (terrestrial  or  aerial,  manned  or  unmanned) 
in  order  to  share  navigation  information  with  limited  communication  bandwidth.  The  work  pre¬ 
sented  in  this  document  builds  upon  the  existing  state-of-the-art  in  related  work  stemming  from 
single -robot  navigation,  terrestrial  cooperative  localization,  underwater  navigation,  and  planning 
under  uncertainty.  The  most  salient  features  of  the  prior  literature  are  discussed  below. 

1.2  A  Review  of  Single  Robot  Navigation 


Figure  1.1  Illustration  of  the  full  SLAM  problem.  A  SLAM  solution  computes  the  unknown  robot  and  landmark  poses 
represented  by  circles  and  stars,  respectively,  given  noisy  observations  illustrated  by  edges. 


One  of  the  fundamental  challenges  in  robotics  lies  in  answering  the  question  ‘where  am  I?’ . 
The  problem  of  concurrently  building  a  model  of  an  a  priori  unknown  environment  and  localizing 
within  it  has  been  addressed  by  robotics  researchers  over  the  last  several  decades  by  the  develop¬ 
ment  of  simultaneous  localization  and  mapping  (SLAM)  frameworks.  Since  we  are  interested  in 
underwater  localization,  we  focus  on  metric  SLAM  formulations.  This  class  of  algorithms  com¬ 
putes  a  joint  estimate  over  robot  poses  and  a  map — generally  a  collection  of  landmarks  or  features. 
The  SLAM  solution  is  often  partitioned  into  two  sub  problems:  inference  (i.e.,  optimization)  and 
data  association  (i.e.,  recognizing  landmarks),  known  as  the  back-end  and  front-end  problems, 
respectively.  Here,  we  only  review  the  back-end  SLAM  problem  as  it  relates  to  cooperative  local¬ 
ization  and  assume  that  the  identities  of  robots  and  landmarks  are  known. 

In  general,  the  full  metric  SLAM  solution  is  provided  by  the  maximum  a  posteriori  (MAP) 
estimate  over  a  set  of  poses  X  =  (xi, . . . ,  xn}  and  landmarks  L  =  {li, . . . ,  lm}  given  the  set  of 
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control  inputs  U  =  {ui, . . . ,  un_!}  and  observations  Z  =  {zu  . . . ,  zp} 

X,  L  =  argmaxp(X,  L|U,  Z),  (1.1) 

XL 

as  illustrated  in  Fig.  1.1.  In  the  following  sections,  we  review  various  solutions  to  the  general 
problem. 

1.2.1  Filtering  Approaches 

The  pioneering  SLAM  formulation  by  Smith  et  al.  (1986)  began  to  address  the  problem  of  fusing 
uncertain  spatial  information  from  sensors  of  varying  quality  for  robot  navigation.  Their  solution, 
among  others  including  Leonard  and  Durrant-Whyte  (1991),  tracks  the  so-called  stochastic  map, 
a  map  composed  of  various  landmarks  such  as  corners,  edges,  or  other  fiducial  markers  scattered 
throughout  the  environment.  The  robot  adds  landmarks  to  the  map  relative  to  its  current  pose  as  it 
travels.  Subsequent  re-observation  of  the  initialized  landmarks  provides  a  constraint  on  the  robot’s 
pose  as  a  loop  closure  event.  The  power  of  SLAM  algorithms  rests  in  the  ability  to  identify  and 
incorporate  these  events,  thereby  strengthening  map  relationships. 

Early  SLAM  solutions  recursively  track  a  state  vector  composed  of  the  current  robot  pose  and 
landmark  positions,  x  =  [x;' .  1  ,r \Jn]T ,  within  a  Bayesian  estimation  framework,  e.g.,  the 
extended  Kalman  filter  (EKF).  The  filtering  solution  corresponds  to  the  Bayes  filter  formulation  of 
(1.1).  Given  linear  Gaussian  noise  models,  the  Kalman  filter  provides  the  optimal  minimum  mean 
squared  error  (MMSE)  estimate.  System  nonlinearities  are  dealt  with  in  ad  hoc  estimators  such 
as  the  EKF  by  repeated  linearization  or  the  unscented  Kalman  filter  (UKF)  by  applying  the  un¬ 
scented  transform.  The  posterior  distribution  over  state,  however,  is  assumed  to  be  approximately 
Gaussian,  x  ~  A/Y/x,  E)  with  mean  p  and  covariance  E.  Filtering  complexity  generally  grows 
with  state  size  because  map  updates  involve  expensive  matrix  operations  (in  general,  matrix  mul¬ 
tiplication  is  0(n3)  in  the  state  dimension).  Additionally,  the  fully  dense  covariance  matrix  has  a 
quadratic  memory  requirement. 

The  information  filter  (Bar-Shalom  et  al.,  2001)  is  the  information  (inverse  covariance)  form  of 
the  Kalman  filter,  x  ~  A/”-1  (77,  A) ,  where  77  and  A  are  the  information  vector  and  matrix,  respec¬ 
tively.  Thrun  et  al.  (2004)  empirically  observed  that  the  information  matrix  over  landmark  states  is 
dominated  by  a  few  terms  that  encode  relationships  between  nearby  landmarks,  suggesting  that  the 
dense  information  matrix  can  be  approximated  as  a  sparse  matrix.  By  performing  a  sparsification 
procedure,  the  authors  improved  upon  previous  EKF  based  implementations,  allowing  constant 
time  updates  and  memory  requirements  linear  in  the  number  of  map  landmarks.  The  sparsification 
procedure,  however,  introduced  the  potential  for  overconfidence  as  it  throws  away  information, 
i.e.,  ignores  conditional  relationships  between  map  elements. 
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Particle  filters  (Doucet  et  al.,  2001)  are  also  a  popular  tool  for  filter-based  SLAM  (Montemerlo 
et  al.,  2002).  Unlike  the  previously  mentioned  methods,  the  particle  filter  is  able  to  represent  ar¬ 
bitrary  posterior  distributions — not  only  unimodal  Gaussian.  However,  the  complexity  of  particles 
filters  is  difficult  to  evaluate  due  to  the  ambiguity  in  defining  adequate  particle  density. 

1.2.2  Graph-based  Approaches 

The  challenge  of  the  data  association  problem  is  defining  a  set  of  repeatably  observable  landmarks 
or  features.  Lu  and  Milios  (1997)  proposed  a  SLAM  solution  that  copes  with  this  problem  through 
an  alternate  map  representation:  modeling  the  environment  as  a  collection  of  robot  poses  or  views 
as  opposed  to  a  set  of  landmarks  or  features.  The  collection  of  historic  robot  poses  is  commonly  re¬ 
ferred  to  as  the  pose-graph.  Lu  and  Milios  (1997)  showed  that  the  pose-graph  formulation  leads  to 
an  equivalent  optimization  problem  that  minimizes  the  error  of  reprojected  data  between  overlap¬ 
ping  views.  Although  their  original  solution  solved  a  batch  maximum  likelihood  estimate  (MLE) 
problem,  Gutmann  and  Konolige  (1999)  later  presented  a  real-time  implementation  that  essentially 
performs  a  batch  update  over  a  sliding  window  of  robot  poses.  Several  derivative  works  employ 
an  equivalent  ‘delayed-state’,  i.e.,  view-based,  filtering  framework  (Eustice  et  al.,  2006a;  Walter 
et  al.,  2007)  to  achieve  real-time  performance. 

Based  on  the  work  of  Lu  and  Milios  (1997)  and  Thrun  et  al.  (2004),  Eustice  et  al.  (2006a)  ob¬ 
served  that  the  information  matrix  of  a  delayed-state  representation  is  exactly  sparse,  i.e.,  requires 
no  approximate  sparsification  procedure.  The  authors  noted  that  the  information  matrix  becomes 
fully  dense  as  a  result  of  marginalizing  out  robot  poses,  whereas  maintaining  the  robot  trajectory 
preserves  sparsity.  The  distribution  over  the  delayed-state  vector  can  be  factored  as  a  Markov 
random  field  (MRF)  and  represented  graphically  (Fig.  1.2a).  The  information  matrix  exactly  cor¬ 
responds  to  the  adjacency  matrix  of  the  the  MRF.  Marginalization  introduces  new  edges  in  the 
graph  called  fill-in,  which  generally  increases  the  graph  density  and  therefore  the  complexity  of 
the  solution.  This  insight  allows  the  benefits  of  constant  time  updates  without  the  drawbacks  of 
an  approximate  sparsification  that  can  lead  to  overconfidence.  Furthermore,  Eustice  et  al.  (2006a) 
noted  that  the  information  form  encodes  a  sparse  linear  system  in  which  most  updates  involve  only 
a  small  portion  of  the  system,  suggesting  a  fast  approximate  method  for  state -recovery. 

Filtering  solutions  all  suffer  from  eventual  divergence  that  occurs  as  a  result  of  linearization 
error  (Julier  and  Uhlmann,  2001).  Lu  and  Milios  (1997)  computed  the  full  robot  trajectory  as 
the  solution  to  a  sparse  nonlinear  system  over  robot  poses — minimizing  the  squared  error  over 
robot  poses.  Similarly,  the  full  SLAM  problem  defines  a  sparse  system  over  both  robot  poses 
and  landmarks  as  in  Fig.  1.2.  Graphical  SLAM  by  Folkesson  and  Christensen  (2004),  Square 
root  smoothing  and  mapping  (SAM)  by  Dellaert  and  Kaess  (2006),  and  GraphSLAM  by  Thrun 
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Figure  1.2  The  graphical  representation  of  the  SLAM  problem  (Fig.  1.1)  exposes  its  sparse  underlying  structure. 


(a)  Information  matrix  and  MRF  associated  with  the  full  SLAM  problem. 


(b)  Measurement  Jacobian  and  factor  graph  associated  with  the  full  SLAM  problem.  Small  circle  nodes 
represent  factors,  i.e.,  measurements.  The  factor  graph  corresponds  to  a  least-squares  problem  that 
computes  the  parameter  vector,  0,  in  order  to  best  explain  the  observations. 


and  Montemerlo  (2006)  considered  the  efficient  solution  of  this  system  using  the  Gauss-Newton 
algorithm,  involving  iteratively  solving  a  large  sparse  linear  system.  Current  state-of-the-art  SLAM 
algorithms  leverage  well  established  sparse  linear  algebra  routines  to  quickly  and  efficiently  solve 
the  underlying  large  linear  system  (Dellaert  and  Kaess,  2006;  Olson  et  al.,  2006b;  Kaess  et  al., 
2008,  2012;  Kummerle  et  al.,  2011;  Polok  et  al.,  2013). 

For  additive  Gaussian  noise  models,  the  MAP  SLAM  formulation  (1.1)  can  be  written 
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where  the  parameter  vector  ©  includes  poses  X  and  landmarks  L.  and  /*(  • )  and  hL{  ■ )  are  the 
process  and  observation  models  with  noise  covariance  matrices  Q,  and  R;,  respectively.  We  rec¬ 
ognize  (1.2)  as  a  nonlinear  least-squares  problem.  Within  a  nonlinear  least-squares  solver  (e.g., 
Gauss-Newton),  (1.2)  is  linearized  to  produce  a  large  linear  least-squares  problem  in  the  estimate 
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where  the  matrix  A  represents  the  stacked  whitened  measurement  Jacobian  and  A  =  ATA  is  the 
information  matrix  (inverse  error  covariance  matrix,  i.e.,  E_1).  Probabilistic  graphical  models 
corresponding  to  this  problem  are  discussed  in  Appendix  A. 

Algorithms  to  solve  linear  systems  are  classified  as  direct  methods  (e.g.,  matrix  factorization 
as  in  Cholesky,  LU,  or  QR),  or  iterative  methods  (e.g.,  relaxation  or  conjugate  gradients).  Direct 
methods  are  more  common  for  SLAM  solutions.  Dellaert  and  Kaess  (2006)  considered  direct  fac¬ 
torization  methods  (LU  and  QR)  in  their  Square  root  SAM  algorithm.  Kaess  et  al.  (2008)  proposed 
recursively  updating  the  QR  factorization  within  the  incremental  smoothing  and  mapping  (iSAM) 
framework.  Direct  solution  methods  rely  on  variable  elimination  (equivalent  to  marginalization). 
As  such,  an  optimal  variable  ordering  exists  which  reduces  fill-in  as  variables  are  eliminated,  there¬ 
fore  preserving  sparsity  and  boosting  performance.  Both  Square  root  SAM  and  iSAM  exploit 
efficient  variable  ordering  heuristics  to  speed  up  inference. 

Iterative  linear  solvers  offer  several  advantages  over  direct  methods  in  terms  of  memory  man¬ 
agement,  parallelization,  and  distribution.  However,  they  may  require  many  iterations  for  conver¬ 
gence.  Duckett  et  al.  (2000,  2002)  proposed  a  Gauss-Seidel  relaxation  based  SLAM  algorithm 
that  was  capable  of  producing  consistent  maps.  Both  Howard  et  al.  (2001)  and  Frese  et  al.  (2005) 
considered  multi-grid  relaxation  methods  popularized  in  computational  mechanics  for  the  solu¬ 
tion  of  large  partial  differential  equations.  Relaxation  based  methods  unfortunately  have  no  upper 
bound  on  the  number  of  iterations  required  to  reach  a  desired  tolerance  of  a  fixed-point  solution. 
Conjugate  gradients  only  requires  a  finite  number  of  iterations  to  converge,  but  cannot  guaran¬ 
tee  that  intermediate  steps  monotonically  approach  the  minimum.  Moreover,  the  required  number 
of  iterations  may  still  be  prohibitively  large.  Thrun  and  Montemerlo  (2006)  suggested  conjugate 
gradients  for  a  batch  solution,  while  Konolige  (2004)  implemented  a  real-time  iterative  method 
based  around  conjugate  gradients  similar  to  Gutmann  and  Konolige  (1999).  Dellaert  et  al.  (2010) 
proposed  a  special  preconditioned  conjugate  gradients  method  for  large  scale  SLAM  problems 
that  outperforms  direct  methods  for  dense  maps.  Preconditioning  is  extended  to  extremely  large 
structure-from-motion  (SfM)  problems  (analogous  to  the  batch  full  SLAM  problem)  by  Agarwal 
et  al.  (2010)  and  Jian  et  al.  (2012,  2013). 
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1.3  A  Review  of  Cooperative  Navigation 


Figure  1.3  The  graphical  representation  of  cooperative  SLAM  illustrates  the  sparse  structure  as  well  as  the  origin  of 
information,  i.e.,  edges  and  factors  are  colored  by  their  origin  platform.  ©  includes  poses  of  all  platforms,  while  no 
landmarks  are  present  in  this  example — observations  are  only  directly  between  platforms. 


(a)  Information  matrix  and  MRF  associated  with  the  cooperative  SLAM  problem. 


(b)  Measurement  Jacobian  and  factor  graph  associated  with  the  cooperative  SLAM  problem. 


Fielding  teams  of  robots  promises  to  boost  reliability  and  performance  over  single  robot  de¬ 
ployments.  Additionally,  information  can  be  shared  across  the  team  to  improve  navigation.  Similar 
to  single -robot  SLAM,  cooperative  multiple-platform  SLAM  is  interested  in  answering  the  ques¬ 
tion  ‘where  are  weT .  These  algorithms  fuse  information  originating  from  several  platforms  includ¬ 
ing  relative  vehicle  observations  in  addition  to  odometry  and  landmark  measurements.  Cooperative 
SLAM  imposes  a  larger  computational  cost  (as  the  state  dimension  grows  linearly  with  the  number 
of  platforms)  and  increased  communication  cost  when  compared  to  single  robot  SLAM.  Addi¬ 
tionally,  cooperative  SLAM  algorithms  may  have  to  determine  the  initial  correspondence  between 
local  reference  frames  defined  for  each  separate  platform.  For  the  cooperative  AUV  algorithms 
presented  in  this  thesis,  all  platforms  are  initialized  in  the  same  local  reference  frame  given  GPS 
observations  at  the  surface. 

The  multi-platform  SLAM  problem  can  be  solved  as  a  batch  process  within  a  centralized  esti- 
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mator  as  suggested  by  Howard  et  al.  (2002)  and  Dellaert  et  al.  (2003)  using  an  MLE  framework. 
A  centralized  system  has  direct  access  to  all  measurements  as  they  become  available  and  hence 
provides  the  benchmark  solution.  This  solution  corresponds  to  (1.1)  where  all  vehicle  poses  are 
now  included  as  illustrated  in  Fig.  1.3.  Centralized  solutions,  however,  are  not  generally  attainable 
online  due  to  their  large  communication  and  processing  requirements.  Decentralized  solutions  are 
necessary  for  real-time  implementation  within  the  underwater  domain  given  the  constraints  of  the 
acoustic  communication  channel  (Section  1.5).  Below,  we  classify  distributed  systems  as  a  su¬ 
perset  of  decentralized  in  that  distributed  systems  may  require  some  centralized  coordination  (Mu 
et  al.,  2011). 

1.3.1  Distributed  SLAM 

Early  work  on  decentralized  cooperative  networks  by  Kurazume  et  al.  (1994)  and  Rekleitis  et  al. 
(2000)  assigned  alternating  dynamic  and  static  roles  to  each  robot  team  member.  Each  moving 
team  member  performs  single-robot  SLAM  treating  the  stationary  robots  as  landmarks.  Although 
these  methods  do  not  recreate  the  centralized  benchmark,  they  do  not  require  any  (or  have  little) 
communication  overhead  and  greatly  reduce  the  rate  of  error  accumulation.  However,  error  growth 
is  still  unbounded  in  time  since  the  ‘landmarks’  must  be  re-initialized  each  time  the  role  designation 
switches.  Moreover,  this  scheme  slows  the  overall  rate  of  team  movement  as  several  team  members 
are  static  at  any  given  time  vitiating  several  benefits  of  multiple  vehicle  operations. 

Egocentric  algorithms,  e.g.,  Fox  et  al.  (2000),  consider  the  problem  of  strictly  localizing  a  vehi¬ 
cle  given  the  estimated  pose  of  another  vehicle  and  a  relative  pose  measurement.  These  algorithms 
apply  the  naive  Bayes  assumption,  i.e.,  that  all  vehicle  estimates  are  independent,  and  are  so-called 
egocentric  since  they  track  only  their  own  state  estimate.  After  incorporating  information  from 
another  platform,  however,  the  two  vehicle  states  are  dependent.  At  the  next  measurement  update, 
the  vehicles  (erroneously)  assume  that  their  estimates  are  independent.  These  methods  incur  very 
low  communication  cost  as  only  a  single  pose  vector  and  covariance  ever  need  to  be  transmitted. 
Egocentric  methods  ignore  dependency,  and  are  inconsistent  (overconfident)  as  a  result,  but  are 
trivially  tolerant  to  communication  failure.  Problems  associated  with  ‘double-counting’  informa¬ 
tion  are  a  hallmark  of  the  cooperative  localization  literature.  A  consistent  estimate  is  one  that  does 
not  produce  an  overconfident  covariance  estimate  (Bar-Shalom  et  al.,  2001),  i.e.,  E  A  E,  where  E 
and  E  are  the  estimated  and  true  covariance,  respectively. 

Roumeliotis  and  Bekey  (2002)  first  noted  that  ignoring  dependencies  that  develop  between 
estimates  can  lead  to  gross  inconsistency  and  overconfidence.  They  proposed  a  distributed  Kalman 
filter  approach,  which  accounts  for  the  interdependency  of  information  at  the  cost  of  significantly 
increased  communication.  Thrun  and  Liu  (2003)  presented  the  distributed  version  of  the  sparse 
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extended  information  filter  (SEIF),  which  is  similar  in  spirit  to  Roumeliotis  and  Bekey  (2002), 
although  it  performs  a  sparsification  procedure  to  reduce  complexity.  These  methods  distribute 
the  full  computation  of  the  centralized  filter,  which  is  guaranteed  to  be  consistent  as  it  accounts 
for  all  information,  amongst  the  vehicle  network.  Nerurkar  and  Roumeliotis  (2008)  and  Nerurkar 
et  al.  (2009)  developed  a  distributed  nonlinear  MAP  estimator.  The  authors  leverage  a  distributed 
conjugate  gradients  algorithm  (Bertsekas  and  Tsitsiklis,  1997)  to  distribute  the  computation  of  the 
centralized  linear  subproblem. 

Instead  of  attempting  to  reconstruct  a  centralized  filter,  Bahr  et  al.  (2009b)  proposed  a  book¬ 
keeping  strategy  to  prevent  fusing  correlated  data,  termed  the  interleaved  update  (IU)  algorithm. 
While  IU  avoids  inconsistency,  the  communication  cost  is  significantly  higher  than  its  egocentric 
counterpart  as  it  maintains  and  communicates  a  bank  of  filters,  which  grows  exponentially  in  the 
size  of  the  vehicle  network.  Moreover,  IU  only  uses  a  subset  of  relative  platform  information  as  it 
periodically  discards  correlated  information. 

Decentralized  data  fusion  (DDF)  for  general  multiple  sensor  systems  has  recently  informed 
cooperative  navigation  research.  The  key  philosophy  of  DDF  is  that  locally  obtained  information 
is  first  fused  locally  and  then  communicated  where  it  is  fused  with  all  accumulated  messages  from 
the  network.  In  this  way,  each  platform  has  a  consistent  estimate  that  is  tolerant  to  communica¬ 
tion  failure  using  all  received  information.  Covariance  intersection  (Cl)  presented  by  Julier  and 
Uhlmann  (1997),  later  expanded  by  Mahler  (2000)  and  generalized  by  Bailey  et  al.  (2012),  sug¬ 
gests  a  conservative  approach  for  combining  estimates  with  unknown  dependence.  Grime  et  al. 
(1992)  reported  the  channel  filter,  which  is  capable  of  distributing  the  linear  information  filter  for 
known  communication  topologies.  In  the  case  of  DDF  frameworks,  each  locally  computed  esti¬ 
mate  will  not  exactly  match  the  centralized  solution  because  each  platform  may  have  only  received 
a  subset  of  the  information. 

Graph-based  cooperative  localization  research  shares  the  DDF  mentality  by  combining  a  local 
and  global  fusion  layer.  As  such,  many  algorithms  require  that  each  platform  perform  the  following 
three  tasks: 

i.  Build  a  local  graph  fusing  local  information  (as  in  the  single  robot  SEAM  scenario), 

ii.  Communicate  and  collect  local  graphs  from  other  platforms, 

iii.  Optimize  the  global  graph  that  fuses  the  received  local  graphs. 

The  decentralized  data  fusion  smoothing  and  mapping  (DDF-SAM)  algorithm,  proposed  by  Cun¬ 
ningham  et  al.  (2010,  2012,  2013)  explicitly  performs  exactly  these  tasks  within  the  iSAM  frame¬ 
work.  Each  robot  builds  a  local  graph  just  as  in  single  robot  SEAM  and  communicates  it  across 
the  network.  Concurrently,  each  robot  collects  local  graphs  from  each  of  its  teammates.  The  set 
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of  received  local  graphs  is  then  optimized  in  a  single  framework.  These  methods  attempt  to  recon¬ 
struct  the  centralized  graph  (Fig.  1.3).  Unfortunately,  the  communication  cost  grows  with  the  size 
of  each  local  graph. 

The  DDF  model  for  distributed  robot  localization,  such  as  in  Cunningham  et  al.  (2010),  remains 
the  standard.  Kim  et  al.  (2010)  report  on  the  alignment  of  multiple  pose-graphs  using  iSAM.  They 
employ  the  ‘anchor-node’  concept,  equivalent  to  the  base-nodes  of  Ni  et  al.  (2007),  to  simultane¬ 
ously  compute  the  initial  correspondence  between  robot  reference  frames.  Howard  et  al.  (2003) 
distribute  their  earlier  centralized  MLE  approach  (Howard  et  al.,  2002)  using  a  system  similar  to 
relaxation.  The  advantage  to  these  solutions  is  that  the  estimate  is  guaranteed  to  be  consistent 
because  the  full  pose  history  is  maintained. 

The  above  algorithms  may  consider  a  communication  limit,  but  do  not  constrain  it  to  the  point 
of  affecting  performance.  Ribeiro  et  al.  (2006)  suggested  a  noteworthy  approach  to  multi-sensor 
data  fusion,  transmitting  a  single  bit  per  measurement,  the  sign-of-innovations.  Within  this  frame¬ 
work,  each  platform  attempts  to  approximate  the  centralized  estimate  using  each  quantized  mea¬ 
surement.  The  authors  derive  an  algorithm  that  closely  mirrors  the  standard  Kalman  filter  by  ap¬ 
proximating  the  posterior  probability  given  each  quantized  measurement  as  a  Gaussian.  Nerurkar 
et  al.  (2011b)  elaborated  on  this  work  by  allowing  local  analog  (not  quantized)  measurements  in 
addition  to  the  remote  quantized  observations.  Trawny  et  al.  (2009)  presented  a  MAP  estima¬ 
tor  integrating  sign-of-innovations  quantized  observations.  Although  there  is  a  small  bandwidth 
requirement,  these  algorithms  require  a  non-lossy  all-to-all  communication  topology. 

Leung  et  al.  (2010)  attempted  to  reduce  the  required  bandwidth  of  communicated  information 
by  leveraging  the  Markov  structure  of  local  graphs.  This  approach  greatly  reduces  the  required 
bandwidth,  although  relies  on  acknowledgments  to  bound  communication.  Without  acknowledg¬ 
ment,  the  required  communication  may  grow  too  large. 

1.3.2  Submap  Approaches 

Submap-based  SLAM,  multiple  session  SLAM,  and  the  multiple  platform  cooperative  localiza¬ 
tion  problem  are  in  many  ways  strongly  related.  Submap  and  multiple  session  approaches  seek  to 
align  several  maps  originating  from  a  single  robot  at  different  times,  whereas  multiple  platforms 
may  align  several  maps  from  different  robots  collected  simultaneously.  In  order  to  efficiently  cope 
with  large  environments,  many  single-robot  SLAM  algorithms  employ  submap-based  distribution 
schemes  such  as  ATLAS  SLAM  (Bosse  et  al.,  2004;  Newman  et  al.,  2002).  A  submap-based  ap¬ 
proach  essentially  performs  SLAM  within  small  regions  of  the  environment  to  remain  tractable, 
then  performs  a  separate  optimization  step  to  align  each  of  the  submaps.  Multiple  session  algo¬ 
rithms  (Ozog  et  al.,  2015)  may  be  employed  to  support  long-term  single-robot  applications. 
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Ni  et  al.  (2007)  proposed  a  submap-based  SAM  approach  termed  Tectonic  SAM.  The  authors 
partition  the  measurement  Jacobian  by  metric  submaps,  and  perform  the  optimization  first  over 
submaps  and  then  over  the  map  separators.  The  global  alignment  can  be  performed  efficiently  by 
adding  ‘base-nodes’ — variables  that  dictate  the  relative  coordinate  frame  of  each  submap.  Base- 
nodes  reduce  the  complexity  of  the  measurement  Jacobian.  Tectonic  SAM  was  then  trivially  ex¬ 
tended  to  a  distributed  multi-robot  application  by  Andersson  and  Nygards  (2008)  in  an  algorithm 
dubbed  collaborative  smoothing  and  mapping  (C-SAM)  wherein  each  submap  was  defined  as  each 
robot’s  local  map.  Each  robot  then  communicates  information  regarding  the  separator  variables 
across  the  network. 

1.3.3  Network  Localization 

Relative  robot  localization  shares  a  high  dot  product  with  network  localization  from  the  distributed 
sensor  network  community.  Researchers  here  often  want  to  localize  static  nodes  given  only  rela¬ 
tive  range  or  bearing  information  to  a  number  of  neighboring  nodes  (Hendrickson,  1995;  Jacobs 
and  Hendrickson,  1997;  Patwari  et  al.,  2005;  Aspnes  et  al.,  2006).  These  algorithms  are  often 
based  on  geometrical  methods  aiming  to  avoid  ambiguities  endemic  to  the  partial-state  informa¬ 
tion  contained  in  range-  or  bearing-only  observations  (Anderson  et  al.,  2008).  Another  body  of 
work  considers  the  optimization  problem  given  a  set  of  range-only  measurements.  In  particu¬ 
lar,  Barooah  et  al.  (2006)  proposes  a  decentralized  relaxation-based  algorithm  for  localization  and 
time-synchronization.  While  these  applications  involve  static  nodes,  a  relaxation-based  method  is 
attractive  for  its  low-bandwidth  requirement  and  ability  to  reconstruct  the  centralized  solution  after 
a  number  of  iterations. 

1.3.4  Consensus 

Consensus  algorithms  have  gained  popularity  in  multiple  platform  systems  for  their  distributive 
properties  (Ren  et  al.,  2005).  Consensus  describes  methods  for  summarizing  and  sharing  informa¬ 
tion  in  order  to  reach  an  agreement.  Other  than  fairly  weak  assumptions  on  network  connectivity, 
communication  in  a  consensus  network  only  depends  on  the  estimated  quantity — independent  of 
the  number  of  platforms,  or  even  the  origin  of  each  message.  In  the  cooperative  control  com¬ 
munity,  agreement  may  refer  to  the  relative  geometry  within  a  vehicle  formation.  Within  sensor 
networks,  agreement  could  pertain  to  estimating  a  particular  quantity,  e.g.,  ocean  temperature  or 
salinity  (Lynch  et  al.,  2008).  Peterson  and  Paley  (2013)  proposed  a  consensus-information  filter 
for  estimating  flow-field  parameters  in  mobile  networks  based  on  the  distributed  Kalman  filter  by 
Olfati-Saber  (2007). 
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Aragues  et  al.  (2012)  proposed  a  multiple  platform  consensus-based  SLAM  formulation  to 
estimate  the  global  landmark  map.  Within  a  cooperative  localization  context,  instantaneous  vehicle 
poses  involved  in  relative  measurements  could  be  considered  landmarks.  The  size  of  the  map,  then, 
would  monotonically  increase  for  each  new  relative  measurement,  quickly  exceeding  allowable 
bandwidth  in  the  underwater  domain.  While  ill-suited  to  the  cooperative  localization  problem, 
consensus  deserves  mention  for  its  low-bandwidth  decentralized  estimation  capabilities  and  use 
within  cooperative  control  architectures. 

1.4  A  Review  of  Underwater  Navigation 


Figure  1.4  AUVs  onboard  the  RJW  Tioga  in  Buzzard’s  Bay,  MA.  The  two  adjacent  AUVs  on  deck  were  used  for 
experimental  validation  throughout  this  thesis. 


Underwater  vehicles  (Fig.  1.4)  typically  integrate  body-frame  velocity  and  attitude  measure¬ 
ments  to  compute  a  dead-reckoned  navigation  solution.  While  depth  and  attitude  are  instrumented 
with  bounded-error,  dead-reckoned  XY-horizontal  position  errors  grow  unbounded  in  time  without 
absolute  position  measurements,  (e.g.,  GPS).  Higher  quality  sensors  are  only  capable  of  reducing 
the  rate  of  error  growth.  Therefore,  alternative  methods  for  constraining  navigation  errors  are  re¬ 
quired.  A  detailed  survey  of  the  state-of-the-art  in  underwater  navigation  is  presented  by  Kinsey 
et  al.  (2006).  We  review  typical  Doppler  navigation  techniques  and  acoustic  beacon-based  naviga¬ 
tion  for  underwater  vehicles. 
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Figure  1.5  Acoustic  navigation  example,  (a)  illustrates  the  trilateration  of  a  vehicle  position  (black  triangle)  given 
ranges  to  known  beacons  (yellow  stars),  (b)  shows  trilateration  using  mobile  beacons  whose  positions  are  not  well 
known. 


1.4.1  Doppler  Navigation 

The  Doppler  velocity  log  (DVL)  is  capable  of  accurately  measuring  seafloor  referenced  (bottom- 
lock)  or  water-column  referenced  (water-lock)  velocity  (RD  Instruments,  1996).  DVLs  observe 
velocity  by  measuring  the  time-dilation  (either  frequency  shift  or  phase)  of  a  reflected  acoustic 
beam.  Vehicles  compute  a  dead-reckoned  navigation  estimate  by  transforming  observed  beam 
velocities  into  the  world  frame  using  observed  attitude  and  then  integrating  in  time.  Whitcomb 
et  al.  (1999a)  report  that  noisy  heading  measurements  are  the  most  significant  source  of  error  for 
bottom-track  DVL  navigation.  For  vehicle  altitudes  in  excess  of  the  bottom-lock  maximum  range, 
water-lock  navigation  is  possible,  although  vehicles  must  also  estimate  the  water  currents.  Without 
an  absolute  position  reference,  dead-reckoned  position  uncertainty  will  grow  unbounded  in  time. 

1.4.2  Fixed-beacon  Acoustic  Methods 

Underwater  acoustic  beacon  navigation  systems  attain  bounded-error  navigation  by  measuring 
their  relative  range  to  beacons  with  known  position  (Vickery,  1998).  These  systems  include:  long- 
baseline  (LBL),  short-baseline  (SBL),  and  ultra-short-baseline  (USBL)  systems. 

Vehicles  observe  their  range  to  a  beacon  by  measuring  the  acoustic  time-of-flight  (TOF)  and 
assuming  a  known  sound  velocity  profile.  Two-way  travel-times  (TWTTs) — round-trip  TOF — are 
obtained  when  a  vehicle  interrogates  and  then  receives  a  response  from  the  beacon  network.  The 
rate  at  which  vehicles  can  receive  range  measurements  decreases  with  the  number  of  vehicles  in  the 
network.  In  contrast,  the  one-way-travel-time  (OWTT)  is  computed  with  knowledge  of  the  exact 
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transmit  and  receive  times;  the  OWTT  is  the  time  difference  between  the  time-of-launch  (TOL) 
and  the  time-of-arrival  (TOA).  This  is  accomplished  by  synchronizing  all  clocks  in  the  network. 
Synchronous-clock  acoustic  networks  can  scale  to  arbitrarily  many  vehicles  because  all  vehicles 
within  acoustic  range  of  the  transmitting  platform  passively  receive  a  range  measurement  leading 
to  constant  time  update  rates.  The  synchronous-clock  hardware  employed  in  this  thesis  is  detailed 
in  Appendix  C. 

The  LBL  navigation  framework,  for  example  in  Fig.  1.5a,  employs  a  network  of  fixed  reference 
beacons  to  which  vehicles  can  measure  range  via  TWTT  (Hunt  et  al.,  1974;  Milne,  1983;  Whit¬ 
comb  et  al.,  1999b;  Yoerger  et  al.,  2007)  and  provides  the  standard  in  underwater  acoustic  position¬ 
ing.  LBL,  however,  limits  the  area  of  operations  to  the  coverage  footprint  of  the  reference  beacons. 
LBL  range  and  accuracy  is  dependent  on  the  acoustic  frequency  employed  since  attenuation  of 
acoustic  signals  is  frequency  dependent.  Low  frequency  systems  (8  —  16  kHz)  can  achieve  a  max¬ 
imum  range  greater  than  10  km,  while  more  accurate  high  frequency  systems  (200  —  300  kHz) 
only  provide  coverage  up  to  a  few  hundred  meters. 

Most  LBL  systems  require  a  lengthy  calibration  procedure  to  survey  the  beacon  network, 
spending  valuable  ship  time  (Jakuba  et  al.,  2008).  Range-only  landmark-based  SLAM  (discussed 
in  Section  1.2)  with  a  priori  unknown  beacon  positions  was  initially  proposed  for  terrestrial  navi¬ 
gation  with  radio  frequency  (RF)  beacons  by  Kantor  and  Singh  (2002)  and  later  expanded  by  Dju- 
gash  et  al.  (2006)  using  the  traditional  EKF.  This  concept  was  later  adapted  to  underwater  acoustic 
networks  by  Newman  and  Leonard  (2003)  and  Olson  et  al.  (2006a).  The  important  insight  in  these 
works  was  to  treat  beacon  TOL  positions  as  landmarks  in  the  SLAM  map. 

Some  recent  work  has  addressed  localization  given  only  a  single  static  beacon  with  known  po¬ 
sition.  The  vehicle  is  not  able  to  trilaterate  its  position  given  a  single  range  observation.  However, 
the  vehicle  can  estimate  its  position  given  multiple  range  observations  to  the  beacon  and  its  dead 
reckoned  navigation.  The  observability  requirements  of  such  a  system  have  been  studied  by  sev¬ 
eral  authors  including  Song  (1999),  Gadre  and  Stilwell  (2005),  Zhou  and  Roumeliotis  (2008),  and 
Fallon  et  al.  (2010b)  using  both  linear  (Chen,  1984)  and  nonlinear  observability  analysis  (Hermann 
and  Krener,  1977).  The  literature  shows  that  only  a  few  trivial  scenarios  exist  that  are  unobserv¬ 
able.  In  general,  if  the  relative  velocity  between  the  platforms  change,  their  relative  position  is 
observable.  In  Chapter  4,  we  consider  the  information  utility  of  relative  vehicle  trajectories. 

1.4.3  Mobile-beacon  Acoustic  Methods 

The  idea  of  single  fixed  beacon  navigation  has  led  to  the  development  of  algorithms  considering 
a  single  mobile  beacon.  Scherbatyuk  (1995)  proposed  an  early  least-squares  algorithm  to  estimate 
the  initial  position  of  an  AUV  and  water  current  given  ranges  to  a  single  beacon  with  known 
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location.  Similar  solutions  using  an  EKF  are  presented  by  Baccou  and  Jouvencel  (2003)  and 
Gadre  and  Stilwell  (2005).  Both  Scherbatyuk  (1995)  and  Baccou  and  Jouvencel  (2003)  note  that 
the  beacon  could  be  another  mobile  vehicle  given  that  it  is  able  transmit  its  own  position  estimate. 
Likewise,  Vaganay  et  al.  (2004)  also  reports  an  EKF-based  solution  where  a  well  instrumented 
AUV  acts  as  a  communication/navigation  aid,  providing  an  accurate  position  estimate  to  a  ‘client’ 
vehicle. 

Recent  algorithms  for  mobile-beacon  acoustic  navigation  are  instances  of  the  more  general 
cooperative  localization  algorithms  reviewed  in  Section  1.3.  The  challenge  for  these  approaches 
is  in  performing  distributed  estimation  over  the  acoustic  channel;  each  platform  must  decide  what 
information  to  share  with  its  team  and  how  to  compute  a  navigation  solution  given  that  information. 

Eustice  et  al.  (201 1),  McPhail  and  Pebody  (2009),  and  Fallon  et  al.  (201 1)  consider  navigating 
an  AUV  given  acoustic  ranges  to  a  ‘server’  surface  ship  using  MLE,  a  nonlinear  least  mean  squares 
method,  and  iSAM,  respectively.  These  methods  consider  each  range  estimate  originating  from  an 
independent  ship  position  estimate,  which  is,  in  general,  only  valid  if  the  position  is  provided  via 
independent  GPS  observations. 

Bahr  et  al.  (2009a)  provide  an  alternative  augmented  navigation  framework  allowing  an  AUV 
to  navigate  given  ranges  to  a  cooperative  navigation  aid  (CNA)  (GPS  reference  source).  This 
algorithm  tracks  all  potential  AUV  positions  as  the  intersection  of  range  circles  propagated  forward 
by  the  AUV’s  dead-reckoning.  Morice  and  Veres  (2010)  suggests  a  similar  geometric  bounding 
technique  to  solve  for  the  initial  AUV  position  based  on  set  membership. 

Maczka  et  al.  (2007)  present  an  egocentric  EKF  (equivalent  to  the  egocentric  approaches  pre¬ 
sented  in  Section  1 .3)  that  is  capable  of  sharing  range  measurements  and  position  estimates  be¬ 
tween  multiple  AUVs.  As  noted  by  Roumeliotis  and  Bekey  (2002)  and  demonstrated  by  Walls 
and  Eustice  (2011),  ignoring  the  correlation  that  builds  between  platform  estimates  given  rela¬ 
tive  observations  leads  to  inconsistency.  On  the  other  hand,  this  approach  is  trivially  tolerant  to 
communication  failure. 

Diosdado  and  Ruiz  (2007)  proposed  a  decentralized  SLAM  information  filter  algorithm  for 
use  with  AUVs.  Each  vehicle  transmits  the  information  matrix  and  vector  corresponding  to  its 
accumulated  landmark-based  map.  This  algorithm  is  similar  to  the  channel  filter  (Grime  et  al., 
1992)  in  that  it  tracks  common  information  in  the  network  in  order  to  maintain  consistency. 

The  centralized  extended  Kalman  filter  (CEKF)  framework  proposed  by  Webster  et  al.  (2009b, 
2012),  similar  to  other  centralized  delayed-state  cooperative  navigation  solutions  (Section  1.3), 
is  particularly  well-suited  for  synchronous-clock  acoustic  navigation.  This  post-process  method 
fuses  all  available  information.  This  work  was  later  decentralized  by  the  authors  (Webster  et  al., 
2010,  2013)  by  exploiting  properties  of  the  information  filter,  but  is  not  feasible  in  practice  without 
modification  as  it  requires  a  non-lossy  communication  channel.  This  method  serves  as  the  basis 
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Table  1.1  Comparison  of  various  underwater  communication  modes. 


Mode 

Frequency 

Range 

Data  Rate 

Long-Wave  Radio 

1  -  100  kHz 

6  —  20  m 

1  kbps 

Optical 

— 

100  m 

>1  Mbps 

Acoustic 

8-28kHz 

100m- 10km 

80  bps-5  kbps  (burst) 

for  the  work  described  in  Chapter  2. 

Fallon  et  al.  (2010a)  suggested  a  bookkeeping  method  dubbed  the  ‘measurement  distribution 
framework’ .  Similar  to  some  DDF  approaches  for  terrestrial  cooperative  navigation,  this  frame¬ 
work  distributes  locally  obtained  map  representations  throughout  a  network.  Unlike  its  terrestrial 
counterparts,  only  the  newest  information  is  transmitted  in  an  effort  to  reduce  the  communication 
cost.  Each  vehicle  tracks  the  information  it  has  received  from  other  platforms  in  a  measurement 
table.  All  contiguous  information  is  then  processed  to  obtain  the  best  available  estimate  over  ve¬ 
hicle  TOA  and  TOL  poses.  Vehicles  have  the  ability  to  request  information  that  is  known  to  have 
been  lost  (deduced  from  the  measurement  table).  This  method  does  not  practically  scale  to  many 
vehicles  because  of  communication  cost;  every  GPS  and  range  measurement  must  be  transmit¬ 
ted  accompanied  by  a  delta  dead-reckoning  from  the  previous  observation  event,  this  includes  all 
TOLs  and  TOAs. 


1.5  A  Review  of  Underwater  Acoustic  Communication 

Multiple  platform  underwater  navigation  relies  on  the  ability  of  each  platform  to  communicate  with 
its  team.  The  physical  characteristics  of  the  underwater  environment  limit  underwater  communica¬ 
tion  compared  to  terrestrial  or  air  (Catipovic,  1990;  Preisig,  2006;  Partan  et  al.,  2007;  Otnes  et  al., 
2012).  The  high  rate  of  absorption  of  both  long- wave  radio  and  optical  signals  in  water  has  limited 
their  use  for  communications  (see  Table  1.1).  Acoustic  communication  improves  upon  these  other 
modes  in  terms  of  range  and  reliability  but  still  faces  several  complications. 

Acoustic  communication  suffers  from  large  propagation  delays  as  a  result  of  the  slow  sound 
speed — roughly  1500  m/s,  five  orders  of  magnitude  slower  than  the  speed  of  light.  Bandwidth 
is  also  extremely  limited  when  compared  to  terrestrial  radio-based  communication  networks.  A 
rough  limit  on  the  range -rate  product  for  acoustic  communication  is  40  km  •  kbps,  which  mostly 
applies  to  vertical  channels  in  deep  water  and  overestimates  performance  in  shallow-water  (Partan 
et  al.,  2007).  Further  physical  difficulties  faced  by  acoustic  communication  include:  absorption 
and  spreading  losses,  waveguide  effects,  multipath,  surface  scattering,  bubbles,  and  ambient  noise 
(Partan  et  al.,  2007).  In  practice,  underwater  vehicle  networks  are  only  able  to  obtain  real-world 
bandwidth  on  the  order  of  100  bps  (Murphy,  2012). 


16 


Most  acoustic  transducers  cannot  simultaneously  transmit  and  receive.  Therefore,  standard 
acoustic  communication  networks  partition  a  transmission  periods  amongst  the  network,  limiting 
each  vehicle’s  opportunity  to  share  information.  Time  division  multiple  access  (TDMA)  is  used 
predominantly  although  frequency-division  multiple  access  (FDMA)  and  code  division  multiple 
access  (CDMA)  are  also  used  with  varying  degrees  of  success.  No  typical  medium  access  control 
(MAC)  protocol  exists,  and  is  usually  designed  application-specific.  The  choice  of  MAC  dictates 
the  ability  of  each  vehicle  to  regularly  transmit  navigation  information. 

1.6  A  Review  of  Robot  Planning 

Planning  has  generally  been  considered  a  separate  problem  from  localization  and  SLAM  and  an¬ 
swers  the  question  ‘how  do  I  get  there?’ .  In  general,  deterministic  start-to-goal  path  planning  is  a 
well  studied  problem  (Latombe,  1991;  Choset  et  al.,  2005;  LaValle,  2006).  Recently,  researchers 
have  begun  to  consider  planning  under  uncertainty  such  that  the  planned  path  reflects  the  uncer¬ 
tainty  in  the  map,  and  may  even  attempt  to  improve  the  map.  For  range-only  cooperative  localiza¬ 
tion,  we  are  interested  in  computing  paths  that  provide  useful  localization  information  to  another 
vehicle  instead  of  simply  a  collision-free  path  to  a  goal  state. 

1.6.1  Deterministic  Robot  Planning 

Path  planning  classically  considers  computing  a  collision-free  path  from  a  start  to  goal  state  given 
a  known  configuration  space.  In  early  planning  work,  the  objective  of  planning  algorithms  was 
completeness — the  ability  to  find  a  collision- free  path  if  one  exists.  These  methods,  however, 
did  not  scale  well  to  high-dimensional  configuration  spaces  and  did  not  explicitly  optimize  any 
performance  metric.  More  recent  planning  algorithms  can  generally  be  categorized  into  three 
classes:  grid  or  resolution-complete,  sampling-based,  or  optimization  motion  planning.  These 
methods  all  assume  that  the  vehicle  has  perfect  state  feedback  and  that  all  controls  will  be  executed 
deterministically.  Examples  of  each  planner  are  illustrated  in  Fig.  1.6. 

Resolution-complete  planners  discretize  the  environment  into  grid  cells  and  apply  search  tech¬ 
niques  such  as  Dijkstra’s  (Dijkstra,  1959)  or  A*  (Hart  et  al.,  1968)  in  order  to  find  optimal  paths 
within  the  grid  environment.  Dolgov  et  al.  (2010)  present  an  A*  approach  for  driving  in  unstruc¬ 
tured  parking  lots.  Resolution-complete  planners  are  generally  intractable  in  high-dimensional 
configuration  spaces.  Using  optimal  search  strategies,  resolution-complete  planners  are  also  reso¬ 
lution  optimal. 

Sampling-based  motion  planning  algorithms  such  as  the  probabilistic  road  map  (PRM)  (Kavraki 
et  al.,  1996;  Svestka  and  Overmars,  1997)  and  the  rapidly-exploring  random  tree  (RRT)  (Lavalle, 
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Figure  1.6  Illustration  of  planning  approaches  for  a  holonomic  vehicle  moving  from  the  start  (green)  to  goal  (red). 


(a)  Grid-based  planner 


(b)  Sample-based  planner  (RRT 
shown) 


(c)  Optimization  motion  plan¬ 
ning 


1998;  LaValle  and  Kuffner,  2001)  can  quickly  explore  high-dimensional  configuration  spaces  and 
offer  probabilistic  guarantees  on  completeness  (completeness  with  probability  one  in  the  limit  of 
infinite  samples).  Elbanhawi  and  Simic  (2014)  provide  a  comprehensive  review  of  sampling-based 
techniques.  Rickert  et  al.  (2014)  noted  that  probabilistic  completeness  does  not  generally  reflect 
the  ability  of  a  planner  to  provide  high  quality  paths.  The  rapidly-exploring  random  graph  (RRG) 
algorithm  and  its  derivative  RRT*  (Karaman  and  Frazzoli,  2011)  provide  both  probabilistic  com¬ 
pleteness  and  asymptotic  optimality  for  a  user  defined  performance  metric.  Essentially,  their  proof 
relies  on  showing  that  in  the  limit,  the  RRG  contains  all  possible  paths  through  the  environment. 

Optimization  motion  planning  and  trajectory  optimization  have  recently  become  popular  due 
to  the  local  optimality  guarantees  they  provide  (Quinlan  and  Khatib,  1993;  Brock  and  Kliatib, 
2002;  Ratliff  et  al.,  2009;  Kalakrishnan  et  al.,  2011;  Zucker  et  al.,  2013).  Sampling-based  planners 
often  include  an  optimization  or  ‘smoothing’  final  step.  Optimization  motion  planning  can  also  be 
performed  more  rapidly  than  sampling-based  planners,  although  initialization  may  be  nontrivial. 

1.6.2  Robot  Planning  under  Uncertainty 

Unfortunately,  both  control  and  sensing  are  nondetermini  Stic,  that  is,  control  actions  cannot  be 
applied  perfectly  nor  the  true  state  known  exactly.  Planning  under  uncertainty  has  been  addressed 
by  planning  in  the  information  state  or  belief  state,  herein  referred  to  as  belief  space  planning. 
Optimal  paths  are  constructed  over  beliefs  (distribution  over  state)  as  opposed  to  the  underlying 
state.  Belief  space  planners  will  often  seek  paths  that  gain  information  (reduce  uncertainty)  in  the 
state.  As  with  deterministic  planning,  solutions  to  belief  space  planning  problems  include  both 
sampling-based  and  optimization  approaches. 

Sampling-based  approaches  to  belief  space  planning  include:  a  simple  breadth-first-search 


18 


graph  search  (Sim  and  Roy,  2005),  roadmap  algorithms  (Prentice  and  Roy,  2009;  Agha-mohammadi 
et  al.,  2014),  RRT  (van  den  Berg  et  al.,  2011a),  and  RRG  methods  (Bry  and  Roy,  2011;  van  den 
Berg  et  al.,  201  la).  These  methods  do  not  explicitly  perform  an  optimization  step,  but  are  able  to 
find  near  optimal  paths  by  searching  over  a  set  of  candidate  solutions. 

Optimal  motion  planning  with  perfect  state  feedback  can  be  formalized  as  a  Markov  deci¬ 
sion  process  (MDP)  given  a  probabilistic  state  transition  model.  With  imperfect  state  information, 
the  belief  space  planning  problem  can  be  framed  as  a  partially  observable  Markov  decision  pro¬ 
cess  (POMDP).  While  POMDPs  are  generally  intractable  (Kaelbling  et  al.,  1998),  several  recent 
methods  have  assumed  parametric  belief  states  (e.g.,  Gaussian)  and  are  able  to  compute  solutions 
in  polynomial  time  (Platt  et  al.,  2010;  van  den  Berg  et  al.,  201  lb,  2012;  Patil  et  al.,  2014;  He  et  al., 
2011).  These  methods  make  linear  Gaussian  approximations  and  rely  on  iterative  linear  quadratic 
Gaussian  (LQG)  or  differential  dynamic  programming  (DDP)  to  solve. 

Active  SLAM  attempts  to  plan  a  path  in  (partially)  unknown  environments  that  trade  off  be¬ 
tween  completing  some  objective  and  reducing  uncertainty  in  its  map.  In  general,  the  active  SLAM 
problem  can  be  expressed  as  a  POMDP.  Recently,  active  SLAM  has  been  framed  within  belief 
space  planning  frameworks  (Sim  et  al.,  2004;  Sim  and  Roy,  2005;  Stachniss  et  al.,  2005;  Kollar 
and  Roy,  2008;  Kim  and  Eustice,  2015;  Chaves  et  al.,  2014;  Indelman  et  al.,  2014,  2015). 

1.6.3  Robot  Cooperative  Control 

Several  researchers  have  proposed  cooperative  adaptive  planning  strategies  in  order  to  optimize 
some  performance  metric.  For  example,  Hollinger  et  al.  (2012)  present  a  path  planning  algorithm 
for  collecting  data  from  a  sensor  network  over  the  acoustic  channel,  i.e.,  data-muling,  onboard  an 
AUV  to  maximize  the  information  collected  while  minimizing  the  travel  cost.  Similarly  Bums 
et  al.  (2006)  and  Leonard  et  al.  (2007)  propose  planning  and  control  methods  to  maximize  com¬ 
munication  performance  and  survey  coverage,  respectively.  Both  Lynch  et  al.  (2008)  and  Peterson 
and  Paley  (2013)  present  cooperative  control  algorithms  to  estimate  unknown  spatial  fields  using 
consensus-information  filters. 

1.6.4  Planning  for  Cooperative  Localization 

Planning  for  range-only  cooperative  localization  has  been  previously  considered  by  several  re¬ 
searchers.  Olson  et  al.  (2006a)  proposed  an  algorithm  for  an  AUV  to  localize  a  fixed  LBL  beacon. 
Charrow  et  al.  (2012)  presented  an  algorithm  to  localize  a  mobile  target  given  radio-frequency 
based  range-only  observations.  Tan  et  al.  (2014)  considered  planning  a  path  for  one  AUV  in  order 
to  localize  another  AUV  along  a  desired  path.  Both  Bahr  et  al.  (2012)  and  Seto  et  al.  (2011)  con¬ 
sidered  a  similar  problem,  but  did  not  assume  that  the  desired  path  was  known  at  planning  time. 
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To  the  best  of  our  knowledge,  no  previous  work  has  considered  uncertainty  in  the  execution  of  a 
path  or  in  the  acoustic  channel. 

1.7  Thesis  Outline 

Cooperation  within  multiple  platform  teams  offers  the  potential  to  improve  performance,  e.g., 
area-coverage  and  efficiency,  while  simultaneously  providing  navigation  information  to  each  team 
member.  These  are  desirable  traits  for  a  scientific  tool,  especially  in  the  underwater  community 
where  a  vast  area  is  yet  unexplored  and  a  high  demand  for  geo-referenced  data  exists.  This  thesis 
considers  two  problems: 

•  Given  a  team  of  underwater  vehicles,  we  explore  how  to  localize  each  team  member  using 
one-way-travel-time  (OWTT)-derived  range  constraints.  This  problem  addresses  the  infor¬ 
mation  that  must  be  exchanged  amongst  the  team  to  produce  a  consistent  navigation  estimate 
(i.e.,  a  solution  that  does  not  double-count  information). 

•  Certain  geometries  of  cooperative  teams  are  better  suited  to  localize  team  members  given 
relative  range  observations,  suggesting  that  vehicle  surveys  can  be  intelligently  planned  to 
minimize  error  growth.  In  this  problem,  we  consider  the  effect  of  relative  vehicle  trajectories 
on  reducing  error  across  the  vehicle  network. 

The  solution  domain  is  extremely  constrained  given  the  low  bandwidth  and  generally  erratic 
nature  of  the  acoustic  communication  channel.  Both  the  estimation  and  control  subproblems  must 
be  tolerant  to  small  infrequent  communication  updates  between  team  members.  The  proposed 
problem  is  to  exchange  information  between  members  of  an  underwater  communication  network 
in  order  to  augment  their  navigation  with  measured  relative  range  and,  given  an  estimation  frame¬ 
work,  compute  minimal  navigation  error  trajectories.  Toward  this  end,  we  have  produced  the 
following  contributions: 

(i)  Developed  an  exact  one-to-many  (server-to-client)  cooperative  localization  algorithm. 

(ii)  Designed  an  approximate  many-to-many  (peer-to-peer)  scalable  localization  framework,  which 
computes  the  best  solution  given  the  subset  of  information  available  at-hand  to  each  platform. 

(iii)  Developed  a  planning  strategy  for  producing  minimal  navigation  error  survey  trajectories. 

1.7.1  Document  Roadmap 

Each  objective  and  contribution  is  detailed  in  the  following  chapters. 
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Chapter  2  We  show  that  by  expanding  the  state  representation  of  a  vehicle  to  include  historic 
poses  (i.e.,  using  a  delayed-state  framework),  we  can  explicitly  track  correlation  that  devel¬ 
ops  between  platform  estimates  as  a  result  of  sharing  relative  observations.  We  provide  an 
efficient  method  to  incrementally  transmit  the  delayed-state  pose  chain,  represented  as  an 
MRF,  over  a  faulty  communication  channel  in  order  to  exactly  reproduce  the  centralized 
solution  to  the  server-to-client  localization  problem. 

Chapter  3  We  exploit  the  factor  structure  of  a  pose-graph  in  order  to  approximately  share  navi¬ 
gation  information  over  a  faulty  communication  channel.  This  method  achieves  a  scalable 
(albeit  approximate)  peer-to-peer  localization  solution,  i.e.,  one  in  which  every  vehicle  in  the 
network  shares  its  local  information  and  uses  information  obtained  from  other  vehicles. 

Chapter  4  We  present  a  framework  to  compute  a  server  trajectory  that  optimally  localizes  a  client 
vehicle  along  a  desired  survey  path.  To  this  end,  we  have  incorporated  both  uncertain  sensing 
and  acting  into  the  planning  framework,  as  well  as  a  probabilistic  model  of  the  acoustic  com¬ 
munication  channel.  We  allow  server  trajectories  to  be  computed  with  a  receding-horizon 
approach  or  an  optimization  of  parameterized  trajectories. 

Chapter  5  We  provide  a  summary  of  contributions  as  well  as  potential  avenues  for  future  work. 

Appendix  A  We  review  probabilistic  graphical  models  as  they  relate  to  efficient  distributed  solu¬ 
tions  to  the  cooperative  localization  problem.  Specifically,  we  review  Gaussian  MRFs  and 
factor  graph  models. 

Appendix  B  We  detail  the  XY  AUV  odometry  model  that  results  from  forward  Euler  integrating 
DVL  and  attitude  observations.  We  also  discuss  the  OWTT  range  observation  model. 

Appendix  C  We  summarize  specialized  hardware  and  equipment  for  synchronous-clock  acoustic 
navigation  as  employed  throughout  our  field  trials.  We  detail  the  acoustic  message  specifi¬ 
cation  for  each  reported  localization  algorithm. 


21 


Chapter  2 

One-to-many  Localization 


This  chapter  considers  the  problem  of  localizing  a  client  vehicle  by  measuring  its  relative  range  to  a 
server  platform1 .  In  general,  the  server-client  cooperative  localization  problem  is  a  distributed  state 
estimation  problem.  Information  sharing  and  fusion  is  nontrivial  since  the  server  and  client  state 
estimates  become  correlated  after  incorporating  relative  range  information.  Moreover,  platforms 
are  restricted  by  severe  communication  constraints  imposed  by  the  underwater  acoustic  channel. 
We  leverage  properties  of  the  MRF  within  a  ‘delayed-state’  framework  in  order  to  achieve  a  con¬ 
sistent  estimate  that  can  be  computed  onboard  the  client. 

Underwater  vehicles  typically  rely  on  fusing  DVL  body-frame  velocities,  attitude,  and  pressure 
depth  observations  to  compute  a  dead-reckoned  navigation  solution.  While  attitude  and  depth 
are  well  instrumented,  there  is  no  easy  method  to  directly  observe  XY-horizontal  position  (GPS 
does  not  work  underwater).  In  this  chapter,  we  report  a  novel  algorithm  enabling  server  vehicles 
to  cooperatively  aid  the  navigation  of  subsea  client  vehicles,  and  which  is  tolerant  to  the  packet 
loss  and  low-bandwidth  that  is  endemic  in  underwater  acoustic  communication  networks.  Our 
algorithm  is  capable  of  bounding  the  position  error  growth  of  the  client  vehicles  to  that  of  the 
server  vehicles. 

Typical  bounded-error  underwater  navigation  methods,  such  as  LBL,  measure  the  relative  range 
between  the  vehicle  and  fixed  reference  beacons  (Milne,  1983;  Whitcomb  et  al.,  1999b).  The 
relative  range  is  typically  measured  using  two-way  TOF  acoustic  broadcasts  and  assuming  a  known 
sound-speed  profile.  Narrowband  LBL  beacon  networks,  however,  are  limited  in  their  ability  to 
scale  to  many  vehicles  because  only  one  vehicle  can  interrogate  the  network  at  a  time.  Moreover, 
the  range  of  vehicle  operations  is  limited  to  the  acoustic  footprint  of  the  beacon  network.  In  the 
same  vein,  USBL  systems  allow  a  topside  ship  to  observe  the  relative  range  and  bearing  of  a  subsea 
vehicle,  but  are  similarly  limited  in  their  ability  to  scale  to  large  vehicle  networks. 

'Portions  of  this  work  have  appeared  in  previously  published  work  including  (Walls  and  Eustice,  2012,  2013, 
2014a). 
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Figure  2.1  Origin  state  method  algorithm  overview:  the  server  (blue)  fuses  its  local  observations  and  adds  delayed- 
state  poses  at  each  time-of-launch  (TOL)  (the  blue  circles).  The  server  uses  our  novel  origin  state  method  to  incre¬ 
mentally  broadcast  its  pose-graph  in  a  fault-tolerant  way.  At  the  time-of-arrival  (TOA)  of  each  received  origin  state 
packet,  the  client  (yellow)  reconstructs  the  server  pose-graph  and  updates  its  estimator  to  fuse  all  new  information.  In 
this  example,  although  the  client  misses  the  server  transmission  at  t%,  the  client  still  reconstructs  the  server  pose-graph 
after  receiving  the  origin  state  packet  at  t.3,  which  encapsulates  all  server  information  accumulated  between  t  \  and  £3. 


The  use  of  synchronous-clock  hardware  enables  a  team  of  vehicles  to  observe  their  relative 
range  via  the  OWTT  of  narrowband  acoustic  broadcasts  (Eustice  et  al.,  2011).  The  OWTT  rela¬ 
tive  range  is  measured  between  the  transmitting  vehicle  at  the  TOL  and  the  receiving  vehicle  at 
the  TOA.  Since  ranging  is  passive — all  receiving  platforms  observe  relative  range  from  a  single 
broadcast — OWTT  networks  scale  well.  The  use  of  OWTT  observations  to  augment  vehicle  navi¬ 
gation  presents  several  open  questions  regarding  how  to  share  and  incorporate  information  across 
the  network  in  a  communication  tolerant. 

The  underwater  acoustic  communication  channel  is  severely  limited  by  the  physical  charac¬ 
teristics  of  seawater  (Partan  et  al.,  2007).  Acoustic  communication  is  constrained  by  high  latency 
and  low  bandwidth  with  packet  loss  often  greater  than  50%.  The  underwater  acoustic  channel  has 
an  upper-bound  range  rate  product  of  40  km  •  kbps.  In  practice,  underwater  vehicle  networks  are 
only  able  to  obtain  real-world  bandwidth  on  the  order  of  100  bps  (Murphy,  2012),  which  is  several 
orders  of  magnitude  less  than  terrestrial  communication  networks.  An  unacknowledged  broadcast 
protocol  is  also  commonly  employed  in  conjunction  with  TDMA  scheduling,  which  further  limits 
overall  bandwidth  by  dividing  transmission  time  between  platforms  in  the  network.  All  of  these 
challenges  amount  to  a  communication  framework  that  enforces  small  communication  data  packets 
and  infrequent  updates  between  vehicles. 

A  variety  of  cooperative  localization  frameworks  exist  for  improving  position  estimates  across 
a  team  of  robots  by  sharing  local  navigation  information.  Previous  methods,  however,  do  not 
address  the  severely  limited  bandwidth  and  fragility  of  the  underwater  acoustic  communication 
channel.  In  this  chapter,  we  consider  a  solution  to  the  navigation  of  a  client  vehicle  aided  by  a 
server  platform,  as  depicted  in  Fig.  2.1.  We  present  the  following  contributions: 

•  We  present  a  general  algorithm,  called  the  origin  state  method  (OSM),  that  allows  a  server 
to  broadcast  its  pose-graph  via  a  faulty,  low  bandwidth  communication  channel  that  works 
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in  real-time  for  practical,  underwater,  acoustic  networks. 

•  We  use  the  OSM  algorithm  to  compute  a  ‘delta  information’  over  server  TOL  poses,  which 
serves  as  input  to  a  decentralized  extended  information  filter  (DEIF)  algorithm  (Webster 
et  al.,  2013)  that  exactly  computes  the  result  of  the  centralized  extended  information  filter 
(CEIF)  onboard  the  client  up  to  communication  delay. 

•  We  provide  extensive  evaluation  over  more  than  12  h  of  field  trials  demonstrating  the  abil¬ 
ity  of  the  OSM  to  broadcast  a  server  pose-graph  to  multiple  clients  using  a  small,  fixed- 
bandwidth  data  packet. 

•  We  provide  a  novel  factor-based  interpretation  of  the  ‘delta  information’  and  discuss  how  it 
can  be  used  in  real-time  in  other  estimation  frameworks  such  as  the  nonlinear  least-squares 
iSAM  framework  (Kaess  et  al.,  2008). 

The  remainder  of  this  chapter  proceeds  as  follows.  Section  2.1  reviews  the  prior  literature 
within  cooperative  localization.  Section  2.2  summarizes  the  proposed  OSM  algorithm.  Section  2.3 
describes  the  interface  between  the  OSM  and  the  DEIF.  Section  2.4  presents  the  results  of  more 
than  12  h  of  field  trials  performed  with  multiple  AUVs.  Finally,  Section  2.6  concludes. 

2.1  Related  Work 

Cooperative  vehicle  networks  enable  robots  with  the  best  navigation  sensors  to  localize  robots  with 
poorer  position  estimates.  The  goal  is  to  augment  each  platform’s  local  sensing  with  measurement 
constraints  between  the  vehicles  themselves  as  depicted  in  Fig.  2.2.  Prior  literature  is  discussed 
below  and  summarized  in  Table  2.1. 

Simple,  real-time  algorithms  that  require  minimal  bandwidth  are  within  the  egocentric  class 
of  filters  (Fox  et  al.,  2000;  Maczka  et  al.,  2007;  Vaganay  et  al.,  2004).  These  algorithms  scale  by 
treating  each  inter-vehicle  relative  observation  as  independent  and  only  require  the  transmitter’s 


Figure  2.2  Joint  pose-graph  (MRF)  over  server,  xS;,  and  client,  xCj,  poses  where  0  is  a  vector  including  server  and 
client  poses,  A  is  the  information  matrix,  and  Tj  is  the  information  vector.  Green  lines  illustrate  edges  generated  by 
relative  range  observations  between  server  TOL  poses  and  client  TOA  poses.  In  order  to  construct  the  full  pose-graph, 
the  client  must  have  access  to  the  server’s  local  information  (shown  in  red). 
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Table  2.1  Summary  of  prior  algorithms  for  multiple  platform  navigation,  specifically  within  the  context  of  server-client  communication  topologies.  No  previous 
method  is  able  to  reproduce  the  centralized  solution  while  being  tolerant  to  communication  packet  loss. 


Literature 

Online/ 

real-time 

Packet  loss 
tolerant 

Bandwidth 
conservative  “ 

Reproduces 

centralized 

Consistent 

estimate 

Comments 

Howard  et  al.  (2002) 

no 

— 

— 

yes 

yes 

Centralized  MLE. 

Centralized 

Dellaert  et  al.  (2003) 

no 

— 

— 

yes 

yes 

Centralized  MLE. 

Webster  et  al.  (2012) 

no 

— 

— 

yes 

yes 

Centralized  EKF. 

Fox  et  al.  (2000) 

yes 

yes 

yes 

no 

no 

Sampling-based  approach. 

Egocentric 

Vaganay  et  al.  (2004) 

yes 

yes 

yes 

no 

no 

Moving  LBL  paradigm. 

Maczka  et  al.  (2007) 

yes 

yes 

yes 

no 

no 

Egocentric  KF. 

Roumeliotis  and  Bekey  (2002) 

yes 

no 

no 

yes 

yes 

Distributed  EKF. 

Ribeiro  et  al.  (2006) 

yes 

no 

yes 

yes 

yes 

Quantized  innovations. 

Bahr  et  al.  (2009b) 

yes 

yes 

yes 

no 

yes 

Bank  of  estimators. 

Fallon  et  al.  (2010a) 

yes 

yes 

no 

yes 

yes 

Requires  acknowledgements. 

Cunningham  et  al.  (2010,  2012) 

yes 

yes 

no 

yes 

yes 

Transmits  reduced  pose-graph. 

Distributed 

Kim  et  al.  (2010) 

yes 

yes 

no 

yes 

yes 

Transmits  entire  server  graph. 

Leung  et  al.  (2010) 

yes 

yes 

no 

yes 

yes 

Transmits  knowledge  sets. 

Webster  et  al.  (2010,  2013) 

yes 

no 

yes 

yes 

yes 

Transmits  delta  information. 

Nerurkar  et  al.  (201  la) 

yes 

no 

yes 

yes 

yes 

Sign-of-innovations. 

Bailey  et  al.  (2011) 

yes 

no 

yes 

yes 

yes 

Transmits  delta  information. 

Origin  State  Method 

(this  chapter) 

yes 

yes 

yes 

yes 

yes 

Can  be  used  in  conjunction  with  EIF  or 
iSAM. 

a  We  consider  an  algorithm  bandwidth  conservative  if  it  employs  a  fixed-bandwidth  data  packet  and  does  not  require 
acknowledgement. 


current  position  estimate.  While  completely  resistant  to  communication  failure,  these  methods  do 
not  account  for  the  correlation  that  develops  through  relative  observations  between  robot  estimates, 
which  can  lead  to  inconsistent  (i.e.,  overconfident)  estimates  (Maczka  et  al.,  2007).  The  negative 
consequences  of  ignoring  correlation  have  been  demonstrated  by  Roumeliotis  and  Bekey  (2002), 
Bahr  et  al.  (2009b),  and  Walls  and  Eustice  (201 1). 

Covariance  intersection  (Julier  and  Uhlmann,  1997)  can  be  used  to  consistently  fuse  two  es¬ 
timates  with  unknown  correlation.  Recently,  it  has  been  applied  to  the  cooperative  localization 
problem  by  Li  and  Nashashibi  (2013)  and  Carrillo-Arce  et  al.  (2013)  to  cope  with  inconsistency 
in  egocentric  algorithms.  However,  previous  work  requires  a  full  rank  (i.e.,  range  and  bearing) 
relative  observation.  Bahr  et  al.  (2009b)  previously  noted  the  challenge  of  incorporating  partial 
relative  pose  information  in  cooperative  frameworks  with  covariance  intersection. 

Bahr  et  al.  (2009b)  and  Fallon  et  al.  (2010a)  proposed  distributed  bookkeeping  strategies  to 
ensure  that  information  is  incorporated  in  a  consistent  manner.  Each  of  their  approaches  requires 
additional  bandwidth  or  use  of  acknowledgments.  Similarly  motivated,  Ribeiro  et  al.  (2006)  and 
Nerurkar  et  al.  (201  la)  achieve  consistency  through  a  noteworthy  approach  in  which  they  transmit 
just  a  single  bit  per  measurement  (representing  the  sign-of-innovations) — yielding  an  algorithm 
that  closely  mirrors  the  standard  Kalman  filter.  While  reducing  overall  bandwidth,  the  algorithm 
requires  full  packet  reception,  which  is  unrealistic  for  faulty  communication  channels. 

The  most  general  cooperative  localization  algorithms  estimate  the  full  joint  distribution  over  all 
vehicle  poses  termed  the  pose-graph  (Fig.  2.2),  and  can  generally  be  realized  through  centralized 
estimators  in  post-process  or  high-bandwidth  systems  in  real-time  (Howard  et  al.,  2002;  Dellaert 
et  al.,  2003).  Roumeliotis  and  Bekey  (2002)  developed  a  distributed  EKF-based  method,  though 
it  requires  moderately  high-bandwidth,  and  two-way  information  exchange.  Cunningham  et  al. 
(2010,  2012)  and  Kim  et  al.  (2010)  studied  the  problem  of  nonlinear  SLAM  in  a  distributed  fash¬ 
ion  using  SAM  where  each  platform  (i)  transmits  its  full  local  pose-graph  (or  a  representative 
subset),  (ii)  collects  the  local  pose-graphs  from  neighboring  platforms,  and  (in)  estimates  the  full 
distribution  by  optimizing  over  all  available  graphs.  The  result  is  a  consistent  estimate  that  matches 
the  centralized  estimator  solution  at  the  expense  of  high  communication  cost,  which  grows  with 
the  size  of  the  local  graph.  Leung  et  al.  (2010)  exploits  the  Markov  property  to  reduce  the  required 
information  exchange  within  a  recursive  Bayesian  filter. 

Webster  et  al.  (2012)  presented  a  post-process  centralized  EKF  for  synchronous-clock  acoustic 
cooperative  localization.  They  later  distributed  this  centralized  filter  result  exactly  (Webster  et  al., 
2013)  with  the  DEIF  by  leveraging  the  sparse  update  properties  of  the  delayed-state  information 
filter.  Their  solution  requires  a  strict  server-client  support  topology,  as  the  server  transmits  repre¬ 
sentative  local  information  (the  ‘delta  state’  information)  to  the  client  where  the  centralized  filter 
solution  is  reproduced.  Bailey  et  al.  (2011)  independently  developed  an  equivalent  formulation 
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for  sharing  locally  obtained  information,  relying  on  fusion  centers  to  perform  relative  robot  mea¬ 
surement  updates.  The  fusion  centers  increase  complexity,  but  allow  for  arbitrary  communication 
topologies.  In  practice,  both  of  these  methods  are  not  practical  in  the  underwater  scenario  because 
they  require  a  non-faulty  communication  channel.  We  previously  reported  (Walls  and  Eustice, 
2012)  a  preliminary  method  toward  alleviating  the  non-faulty  communication  constraint  in  dis¬ 
tributing  local  server  information,  but  which  still  relied  upon  a  client  acknowledgment  scheme — 
limiting  scalability  to  multiple  clients. 

Several  other  works  in  acoustic  cooperative  underwater  navigation  have  emerged  for  fusing 
OWTT -based  relative  ranges  in  server-client  communication  topologies  (Vaganay  et  al.,  2004; 
Maczka  et  al.,  2007;  McPhail  and  Pebody,  2009;  Bahr  et  al.,  2009a,b;  Eustice  et  al.,  2011;  Fal¬ 
lon  et  al.,  2010b,  2011).  Previously  considered  ‘single -beacon’  cooperative  localization  or  the  use 
of  CNAs  are  equivalent  to  the  server-client  scenario  explored  here.  Earlier  methods,  however, 
generally  compromise  between  offline  and  consistent  (by  estimating  the  full  joint  distribution),  or 
real-time  and  inconsistent  (by  ignoring  correlation  between  relative  range  observations).  Note  that 
online  methods  that  neglect  correlation  are,  in  fact,  exact  when  the  server  positions  are  actually 
independent.  While  many  of  these  algorithms  have  been  validated  in  post-process  using  exper¬ 
imental  data,  only  a  few  have  been  presented  with  real-time  field  trials  including  Maczka  et  al. 
(2007),  Fallon  et  al.  (2010b),  and  Fallon  et  al.  (2010a). 

Our  work  is  closest  to  Webster  et  al.  (2013)  and  Bailey  et  al.  (2011)  in  its  effort  to  distribute 
local  data  fusion  and  to  leverage  the  sparsity  of  the  Gaussian  information  form  to  compactly  broad¬ 
cast  this  information.  The  approach  reported  herein  improves  upon  previous  methods  by  (i)  im¬ 
proving  network  scalability  via  a  generally  passive  origin  shifting  scheme  that  eliminates  the  need 
for  client-side  acknowledgments,  ( ii)  introducing  a  recovery  packet  mechanism  that  enables  clients 
to  enter  and  leave  the  network  or  recover  after  a  long  period  of  communication  dropout,  and  (Hi) 
presenting  several  AUV  trials  demonstrating  our  algorithm’s  ability  for  real-time  underwater  nav¬ 
igation. 

2.2  Consistent  Cooperative  Localization 

We  consider  one  or  more  independent  server  vehicles  aiding  the  navigation  of  multiple  client  ve¬ 
hicles,  such  as  is  depicted  in  Fig.  2.3.  For  the  sake  of  presentation,  we  refer  to  a  single  server 
vehicle  throughout  this  section,  although  our  algorithm  can  support  multiple  (the  multiple  server 
scenario  is  demonstrated  in  Section  2.4).  The  client  vehicles  are  able  to  passively  observe  their 
range  to  the  server  vehicle  during  periodic  server  broadcasts.  Each  client  updates  its  pose  estimate 
using  its  local  information,  the  range  observations  to  the  servers,  and  the  information  broadcast  by 
each  server.  A  centralized  estimator,  for  example  Webster  et  al.  (2012),  which  has  access  to  the 
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Figure  2.3  Supported  communication  topology.  One  or  more  server  vehicles  (left,  blue)  broadcast  state  information 
to  a  network  of  client  vehicles  (right,  yellow).  The  OSM  algorithm  allows  each  client  to  reproduce  the  corresponding 
centralized  estimate.  Servers  can  support  many  clients  in  parallel. 


local  and  relative  observations  of  all  vehicles,  but  is  realizable  in  post-process  only,  serves  as  the 
gold-standard  benchmark  solution.  The  centralized  solution  includes  information  from  all  servers 
and  a  single  client  vehicle,  but  not  information  from  the  other  client  vehicles.  Our  formulation  is 
able  to  reproduce  this  centralized  delayed-state  filter  result  onboard  each  client  vehicle  in  real-time 
for  the  server  and  individual  client  states. 

Relative  range  observations  occur  between  the  server  at  the  time-of-launch  (TOL)  and  each 
client  at  the  time-of-arrival  (TOA)  by  measuring  the  one-way-travel-time  (OWTT)  of  an  acoustic 
broadcast.  All  vehicles  synchronize  their  local  clocks  to  GPS  time  while  at  the  surface.  Low- 
drift  reference  clocks  enable  the  vehicles  to  remain  synchronized  throughout  operation.  During 
our  trials  we  used  a  SeaScan  Inc.  temperature  compensated  crystal  oscillator  (TXCO),  which 
provides  a  stable  reference  pulse  with  approximately  1  ms  drift  over  14  h  (Eustice  et  al.,  2011). 
Newer  commercially  available  free -running  clocks  promise  several  orders  of  magnitude  improved 
performance,  for  example,  Symmetricom  (2013)  provides  a  120  mW  chip  scale  atomic  clock  with 
less  than  1  ms  drift  over  5000  h  (~208  days).  Appendix  C  details  the  synchronous-clock  and 
acoustic  modem  hardware. 

Previously  reported  algorithms  that  are  able  to  compute  a  consistent  estimate  track  the  joint 
distribution  over  both  client  and  server  poses,  i.e.,  the  joint  pose-graph  over  the  client  and  server 
(see  Fig.  2.2).  Although  the  client  may  only  be  interested  in  computing  its  own  state  estimate, 
tracking  the  server’s  pose-graph  allows  the  client  to  track  correlation  between  successive  relative 
range  observations.  In  general,  the  server  must  transmit  its  full  local  pose-graph  to  the  client  at  the 
time  of  each  new  relative  measurement  in  order  for  the  client  to  compute  this  full  solution.  Since 
the  size  of  the  server  pose-graph  continually  grows,  transmitting  the  full  server  information  is  not 
feasible  in  a  communication  constrained  domain.  OSM  supplies  a  fixed-bandwidth  representation 
of  new  server  poses  that  allows  each  client  to  asynchronously  reconstruct  the  server  pose-graph 
despite  high  packet  loss.  Each  server  broadcast  contains  all  new  local  information  relative  to  a 
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Figure  2.4  Server  pose-graph  example.  The  gray-shaded  area  of  the  information  matrix  illustrates  that  process  predic¬ 
tions  and  measurements  occurring  between  the  previous  TOL,  xS3,  and  the  current  TOL,  xS4,  contribute  additively  to 
a  subblock  of  the  information  matrix.  The  information  matrix  sparsity  pattern  corresponds  to  the  adjacency  matrix  of 
the  MRF.  The  information  shown  here  exactly  corresponds  to  the  server  information  in  Fig.  2.2. 

a  0  © — © — © — © 

A4  ^4 


server  state  known  by  the  client,  termed  the  origin  state.  The  client  then  reconstructs  the  server 
pose-graph  and  can  compute  the  centralized  solution.  Fig.  2.1  provides  a  graphical  overview  of  the 
OSM  algorithm. 


2.2.1  Information  Filter 

The  OSM  algorithm  relies  upon  manipulating  a  Gaussian  distribution  parameterized  in  the  infor¬ 
mation  form  to  efficiently  broadcast  the  server  pose-graph,  an  MRF.  We  assume  that  the  server 
state  evolves  with  linear  Gaussian  noise  models.  Client  process  and  measurement  models,  how¬ 
ever,  can  be  fully  nonlinear.  The  server  MRF  will  therefore  have  a  known  and  predictable  structure 
that  we  can  leverage  to  broadcast  it  over  an  unreliable  communication  channel.  Although  we  may 
use  any  estimator  that  satisfies  our  assumptions,  we  employ  a  delayed-state  information  filter  to 
initially  construct  the  server  pose-graph. 

The  information  filter  tracks  a  Gaussian  distribution  over  its  state,  x,  parametrized  in  the  in¬ 
formation  form;  that  is  p(x)  =  A/"-1  (x:  rj,  A) ,  where  the  information  vector,  77,  and  matrix,  A,  are 
related  to  the  mean  and  covariance  by 

77  =  A/i  and  A  =  £_1,  (2.1) 


where  //  and  £  are  the  mean  vector  and  covariance  matrix  of  x,  respectively. 

The  single  vehicle  navigation  problem  is  framed  in  terms  of  estimating  the  joint  distribution 
over  a  collection  of  historic  poses  (i.e.,  past  vehicle  states).  In  this  case,  the  state  vector  is  com¬ 
posed  of  these  historic  poses,  termed  ‘delayed-states’, 


x  = 


£-1 
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corresponding  to  the  distribution  p(x),  i.e.,  the  pose-graph.  The  information  filter  state  vector 
grows  over  time  by  performing  prediction  with  augmentation.  As  noted  in  Eustice  et  al.  (2006a), 
processes  that  evolve  sequentially  with  the  Markov  property,  i.e.,  with  distributions  that  can  be 
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factored 


p(x)  =  p(xn|xn_i)  •  •  •p(x2|x1)p(xi), 

admit  a  sparse,  block  tri-diagonal  information  matrix.  This  sparsity  leads  to  an  update  formulation 
that  only  affects  a  small  sub-block  of  the  information  matrix  and  vector,  as  depicted  in  Fig.  2.4. 
New  odometry  inputs  and  local  measurements  only  modify  a  block  of  the  information  matrix  and 
vector  corresponding  to  the  current  robot  pose  and  the  most  recent  delayed-state  (i.e.,  only  the  value 
of  the  pose-graph  edge  between  the  last  delayed-state  and  the  current  state  is  affected).  Herein,  we 
assume  that  new  poses  are  appended  onto  the  state  vector.  Below  we  review  the  filter  mechanics 
originally  presented  by  Eustice  et  al.  (2006a)  that  lead  to  these  sparse  updates. 

Process  Prediction 

Consider  a  state  vector  that  is  composed  of  a  delayed-state  and  the  current  vehicle  state,  xn  = 
[xj,  xj[]T,  with  distribution p(xn \Zn)  parameterized  by  the  information  matrix  and  vector 

A-d,d  ^-d,n 
^-n,d  A-n,n 

V  d 

ln\  ’ 

where  Z n  represents  the  set  of  observations  received  up  to  time  index  n.  The  process  prediction 
computes  the  distribution  p(xn+1\Zn)  given  a  state  transition  model 

X)i+1  =  /(xn,  un)  +  wn, 

where  un  represents  a  control  input  and  w„  ~  A/”(0,  Q)  is  an  independent  noise  disturbance.  The 
information  matrix  and  vector  are  propagated 
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where,  for  notational  convenience, 


O  =  An,n  +  FtQ"1F 
vh  =  (Q  +  FA,-iFT)-1 
*7*  =  Vn  -  FTQ_1(/(M„,un)  -  F/xJ. 

State  augmentation  appends  the  newest  vehicle  state  to  the  state  vector  while  also  maintaining 
the  previous  state  as  a  delayed- state.  In  this  case,  prediction  with  augmentation  simultaneously 
appends  the  new  state  and  predicts  forward 
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We  see  here  that  the  information  update  is  sparse,  as  it  only  affects  the  lower  block  corresponding  to 
the  previous  and  newest  states.  As  mentioned  earlier,  the  cumulative  effect  is  that  the  information 
admits  a  sparse  tri-diagonal  structure  as  more  delayed-states  are  added. 


Measurement  Update 

Consider  the  general  nonlinear  observation  model 


Z)i+ 1  —  A(xn+i)  vn+i, 

where  vn+1  ~  A/”(0,  R)  is  an  independent  noise  perturbation.  The  observation  model  repre¬ 
sents  the  conditional  distribution  p{ zre+i|x„+i).  We  can  then  compute  the  distribution  over  state, 
p(xn+ i|Zn+i),  as  a  simple  additive  update 

An+1  =  An+1  +  HtR_1H 

V n+1  =  'Hn+I  +  HTR-1(zn+1  -  h{fin+ 1)  +  H/xn+1), 

where  H  is  the  observation  Jacobian  and  nn+i  is  the  state  mean  following  process  prediction.  The 
observation  model  generally  only  involves  a  portion  of  the  state  and,  therefore,  the  observation 
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Figure  2.5  Pose-graph  over  server  TOL  poses.  The  OSM  algorithm  allows  the  client  to  reconstruct  the  underlying 
server  pose-graph  (horizontal  lines)  from  the  set  of  received  OSPs  (colored  arcs).  Each  OSP  encodes  the  transition 
from  Xi,  which  represents  the  origin  state. 


OSPj  OSP2  OSP3 


Jacobian  H  is  sparse.  The  information  update  term  is  defined  by  the  outer  product,  1 1  R  1 II. 
which  is  then  also  sparse  and  only  touches  blocks  of  the  information  matrix  corresponding  to 
nonzero  elements  of  the  measurement  Jacobian. 


2.2.2  Origin  State  Method 

Our  goal  is  to  broadcast  information  that  allows  the  client  to  reconstruct  the  server  pose-graph. 
However,  the  server  does  not  have  any  knowledge  of  the  information  that  the  client  has  actually 
received,  because  communication  is  broadcast  and  unacknowledged.  The  underlying  assumption  is 
that  the  server  pose-graph  grows  as  a  Markov  chain — the  standard  model  for  a  dynamical  system. 
The  Markov  chain  is  a  special  case  of  the  more  general  MRF.  Loop  closures,  popular  in  SLAM 
literature,  are  not  supported  by  this  model  since  including  them  in  the  pose-graph  breaks  this  chain 
assumption.  Each  origin  state  broadcast,  called  an  origin  state  packet  (OSP),  encodes  a  server 
transition  from  the  origin  state  to  the  current  state  (Fig.  2.5).  The  OSP  represents  the  relationship 
between  the  origin  and  current  state  as  their  joint  marginal  distribution,  i.e.,  the  two-node  pose- 
graph  over  the  origin  and  current  TOL  states.  We  show  here  that  the  client  can  incrementally 
reconstruct  the  server’s  pose-graph  from  the  sequence  of  received  OSPs. 


Server-side  Origin  State  Operation 


The  server  vehicle  maintains  an  information  filter,  augmenting  its  state  vector  with  a  copy  of  each 
TOL  state.  At  the  TOL,  the  server  broadcasts  an  OSP  containing  the  joint  marginal  informa¬ 
tion  over  the  current  TOL  state  and  a  designated  previous  delayed- state,  the  origin,  as  depicted 
in  Fig.  2.5.  The  index  label  of  the  new  TOF  state  and  the  origin  are  included  in  the  OSP  for 
reconstruction  by  the  client. 

We  can  partition  the  set  of  intermediate  server  TOF  states  occurring  between  the  origin  state, 
x0,  and  the  nth  TOF  state,  xn,  into  the  set  of  states  received  and  not  received  by  the  client,  xr  and 
Xf,  respectively.  The  server  pose-graph  at  the  nth  TOF  then  represents  the  distribution  over  the 
state  vector 
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where  the  ‘s’  superscript  indicates  the  distribution  as  tracked  by  the  server.  Note  that  the  server 
has  no  knowledge  of  the  partition  r  and  f.  This  distribution  is  expressed 
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where  Z,  is  the  set  of  all  observations  up  to  the  /th  time  index.  Note  that  A*  is  block  tri-diagonal 
as  per  the  Markov  chain  assumption. 

Under  the  OSM,  the  server  computes  an  OSP  at  every  TOL,  which  is  simply  the  joint  marginal 
distribution  corresponding  to  the  state  vector 
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computed  via  the  Schur  complement  of  (2.3)  and  (2.4): 


pOSpn(x0,xn|Zn)  /  p  (x0 .  xr,jr .  x/;  |  Zn ) r/xrUr 
=  N  1(*osp„;77osp„)-^osp„)) 


YiOSP„ 

^OSP„ 


As 

0,0 


As 

o,n 


Ai  „  Af 


V  o 


(2.6) 

(2.7) 


This  formulation  allows  the  server  to  remain  ignorant  about  states  the  client  has  received,  r.  More¬ 
over,  it  allows  the  server  to  send  useful  information  to  multiple  clients,  where  each  client  has  a  dif¬ 
ferent  set  of  received  server  TOL  states.  In  order  for  the  client  to  reconstruct  the  server  pose-graph, 
the  client  must  already  have  the  origin  state  in  its  representation,  (i.e.,  the  origin  is  a  previously 
received  TOL  state).  The  index  label  of  the  current  TOL,  n,  and  the  origin,  on,  are  included  in  the 
broadcast. 
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Algorithm  1  Server-side  Origin  State  Method 

Require:  Ao,  r]0  {initial  server  belief} 

1:  A b,  r]b ,  Ob  4—  0  {backup  origin  state  packet} 

2:  loop 

3:  if  k  is  TOLn  then 

4:  A® ,  77® ,  on  <r-  originPacket(Afc,  r)k) 

5:  if  on  /  on- 1  then 

6:  {origin  has  been  shifted,  update  backup  packet} 

7:  Aft,  Oft  A®_1;  <_!,  on_i 

8:  end  if 

9:  broadcast  Or  iginPacket  (A® ,  77® ,  on,  Ab,  r)b ,  Oft) 

10:  if  recoveryRequired()  then 

11:  broadcastRecovery()  {Section  2.2.2} 

12:  end  if 

13:  Afc,  r]k  <-  predict  Augment  (A*.,  r)k) 

14:  n<-n+l 

15:  else 

16:  Afc,  r]k  <-  predict(Afc,  r]k) 

17:  end  if 

18:  A-k:Vk  localMeasUpdate(Afc,  r]k,  zk) 

19:  k<-  k  +  1 

20:  end  loop 


Algorithm  1  summarizes  the  server-side  operation.  The  server  simply  maintains  an  information 
filter  over  its  pose-graph,  augmenting  its  state  vector  with  each  new  TOL  pose.  At  each  TOL  the 
server  computes  an  OSP  to  broadcast  to  the  client.  Origin  shifting  and  recovery  is  introduced  and 
discussed  below. 

Although,  in  general,  the  size  of  the  server  pose-graph  grows  with  the  addition  of  each  new 
TOL  pose,  the  dimension  of  the  OSP  marginal  information  matrix  and  vector  is  fixed.  The  OSP 
dimension  is  twice  the  state  dimension — therefore,  a  minimal  vehicle  state  size  is  desirable  to 
reduce  communication  packet  size. 


Client-side  Origin  State  Operation 

The  client  incrementally  reconstructs  the  server  pose-graph  from  the  sequence  of  successfully 
received  OSPs.  Before  the  nth  TOL,  the  client-side  version  of  the  server  pose-graph  reconstruction 
contains  the  server  states 
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Figure  2.6  Illustration  of  OSM  operation,  (a)  represents  the  server’s  pose-graph  at  the  nth  TOL.  The  server  broadcasts 
an  OSP  (b)  to  the  client.  The  client  has  already  reconstructed  a  portion  of  the  server  graph,  (c),  having  missed  TOL 
poses,  Xf.  The  client  then  reconstructs  the  server  goal  information  illustrated  in  (d)  by  fusion  of  (b)  and  (c).  (a)  and 
(d)  are  equivalent  with  the  unreceived  TOL  states,  xf  marginalized  out. 
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(a)  Server  pose-graph  at  the  current  TOL 
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(b)  Current  OSP 
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(c)  Client-side  reconstruction  through  the  last  received  OSP 


0 - 0—  -0 - 0  p<(x„,xr,x„  |Z„) 

V - V - ' 

xr 

(d)  Client-side  goal  information 


where  the  ‘c’  superscript  indicates  the  server  distribution  as  tracked  by  the  client,  r  =  { r , , . . .  ,rm} 
denotes  the  set  of  received  server  TOL  indices,  rm  is  the  latest  received  TOL  index,  and  r'  = 
{ri, . . . ,  rm_i}  represents  other  previously  received  server  TOL  indices.  To  simplify  notation,  we 
let  m  =  rm  for  the  remainder  of  the  discussion.  Note  that  the  client  has  no  representation  for  server 
states  corresponding  to  missed  TOL  poses,  xf.  This  is  equivalent  to  the  server’s  distribution  at  m 
with  states  in  xf  marginalized  out, 

pc{x0,  xr/,  xm\Zm)  =  J\f~l{xcml  T)cm,  Acm) , 
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After  receiving  the  nth  OSP,  the  client  solves  the  following  problem: 
Given  the  information  available  to  the  client, 


(2.8) 


(2.9) 


1.  pc(xG,  xr|Zm),  the  client-side  server  pose-graph  up  to  the  last  received  TOL  and 

2.  ps(xG,  xn|Zn),  the  new  OSP  (computed  server-side), 

Construct  the  client  goal  distribution,  i.e.,  the  server  pose-graph  up  to  time  n,  as  if  unreceived 
TOL  states  had  been  marginalized  out, 


pc(x0,xr,xn|Zn)  =  JV  1(xcn-,rjcn,  A£)£ 
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(2.10) 


(2.11) 


The  boxed  elements  in  (2.10)  and  (2.11)  indicate  unknown  values  in  the  desired  goal  recon¬ 
struction  while  the  remaining  values  are  known  from  (2.8)  and  (2.9)  because  of  the  assumed 
Markov  structure  for  the  server  states  (block  tri-diagonal  information  matrix).  In  other  words, 
all  new  information  since  the  last  received  OSP  only  affects  a  small  portion  of  the  information 
matrix  and  vector.  The  client-side  reconstruction  is  illustrated  in  Fig.  2.6. 

The  client-side  reconstruction  begins  by  marginalizing  out  xr  from  the  (partially  unknown) 
goal  distribution,  (2.10)  and  (2.11),  producing  an  expression  that  can  be  equated  to  the  (known) 
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received  nth  OSP,  (2.6)  and  (2.7), 


PoSPn  I  I  P  (^o?  |  Zn)c?Xr 

J  xr 

=  PoSP„  (Xo?  xn |^n). 


(2.12) 

(2.13) 


Marginalization  of  the  goal  distribution  via  the  Schur  complement  leads  to  a  set  of  linear  equations 
in  the  unknowns.  We  proceed  with  a  two-step  marginalization  procedure.  First,  we  marginalize 
out  r'  states  from  (2.10)  and  (2.1 1)  via  the  Schur  complement: 
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(2.15) 


where  the  tilde  decoration  indicates  block  elements  that  are  changed  from  (2.10)  and  (2.11).  This 
step  results  in  the  following  set  of  expressions, 
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(2.16) 


Second,  we  marginalize  out  state  m  from  (2.14)  and  (2.15)  and  equate  to  the  server-side  OSP,  (2.6) 
and  (2.7), 
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(2.17) 

(2.18) 


The  unknown  values  of  the  goal  client-side  server  reconstruction,  boxed  in  (2.10)  and  (2.11),  are 
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Algorithm  2  Client-side  Origin  State  Method 

L  -A-o,  Vo  0 

2:  loop 

3:  if  (A*  ,  r/*  ,  on,  A&,  r/;,,  o&)  4-  receivedPacket()  then 

4:  if havePrimaryOriginIndex(on)  then 

5:  A„,  addOriginPacket(A* ,  77*,  on) 

6:  updateDEIF(An,  rjn)  {(2.21),  (2.22)} 

7:  else  if  haveSecondaryOriginlndex(ob)  then 

8:  An_i,  rjn— 1  <-  addOriginPacket(Afe,  r]b,  ob) 

9:  A n,  rjn  4-  addOriginPacket(A* ,  77* ,  on) 

10:  updateDEIF(An,  rjn)  {(2.21),  (2.22)} 

11:  else 

12:  requestRecovery() 

13:  end  if 

14:  end  if 

15:  end  loop 


then  computed  by  substituting  (2.16)  into  the  above  equality 

^m,m 
m,m 
m,n 

V  m 
Vm 

Vn  = 

When  only  one  TOL  state  has  been  received  (i.e.,  m  —  l,r  —  {rq},  and  r'  =  {0}),  the  derivation 
proceeds  as  above,  but  with  only  the  second  marginalization  step. 

As  an  implementation  aside,  at  the  TOA  of  the  first  received  OSP,  the  client  does  not  need 
to  perform  any  computation  to  reconstruct  the  server  pose-graph.  The  initial  OSP  is  simply  the 
two-node  server  pose-graph  consisting  of  the  server  origin  state  and  the  current  TOL  state.  (Note 
that  this  allows  any  new  clients  to  immediately  enter  and  join  the  network,  i.e.,  the  network  can 
dynamically  resize). 

Origin  Shifting 

The  information  difference  (A0)0  —  A*  in  (2.19)  represents  the  delta  information  known  about  the 
origin  state  between  the  client  through  the  last  received  OSP,  pc(x0|xr,  Zm),  and  the  server  at  the 
current  TOL,  ps(xa\xn,  Zn).  This  difference  approaches  machine  precision  as  the  time  between  the 
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origin  and  new  TOL  state  grows  (because  little  additional  smoothing  of  the  origin  state  occurs  after 
sufficient  time).  The  reconstruction  rules  require  the  inversion  of  this  decreasing  term,  leading  to 
numerical  inaccuracies  that  can  cause  divergent  errors  in  the  reconstruction.  A  simple  solution  is 
to  ensure  that  the  origin  is  periodically  shifted  forward. 

We  initially  proposed  an  origin  shifting  scheme  based  on  acknowledgments  from  each  client 
(Walls  and  Eustice,  2012);  however,  an  acknowledgment  based  scheme  does  not  scale  well  to 
many  clients.  Moreover,  numerical  instability  will  continue  to  plague  an  acknowledgement  driven 
system  if  the  server  does  not  regularly  receive  acknowledgments.  Instead,  we  propose  a  shifting 
scheme  in  which  the  server  evaluates  a  function  based  upon  the  numerical  stability  of  the  newest 
OSP — keeping  the  OSP  broadcast  passive  and  not  requiring  any  client  acknowledgements,  such 
that  the  method  can  more  easily  scale  to  many  client  vehicles. 

During  our  real-time  experiments  (Section  2.4),  the  server  shifted  the  origin  forward  using  a 
threshold,  T,  on  the  trace  of  the  difference  term  in  (2.19), 

trace (A0;0  -  A*  0)  <  T.  (2.20) 

The  trace  is  only  used  as  a  measure  to  test  the  numerical  stability  of  the  OSP  and  is  tuned  to 
produce  an  accurate  reconstruction.  Note  that  A0  0  is  the  A*  0  element  from  the  previous  OSP,  and 
is  therefore  readily  available  without  additional  computation. 

When  the  shifting  function  suggests  shifting  the  origin,  the  new  origin  is  set  to  the  previous 
TOL  state.  The  server  is  now  free  to  marginalize  out  TOL  states  preceding  the  new  origin.  To  help 
ensure  that  each  client  vehicle  can  maintain  a  reconstruction  of  the  server  pose-graph  that  contains 
the  origin  state,  in  practice  each  server  broadcast  encodes  at  least  two  OSPs:  the  primary  OSP 
encoding  the  transition  from  the  origin  to  the  current  TOL,  and  a  secondary  OSP  encoding  the 
transition  from  the  previous  origin  to  the  current  origin.  Depending  upon  the  available  bandwidth, 
the  server  could  extend  the  broadcast  to  include  more  than  two  OSPs  to  increase  tolerance  to 
packet  loss.  Prom  an  implementation  standpoint,  the  server  does  not  need  to  recompute  previous 
OSPs,  it  simply  stores  previously  broadcast  messages.  The  server-side  origin  shifting  step  and 
corresponding  client  behavior  are  outlined  in  Algorithm  1  and  Algorithm  2,  respectively. 

After  the  server  shifts  the  origin  forward,  the  client-side  reconstruction  will  contain  server  TOL 
states  preceding  the  new  origin.  The  reconstruction  rules  (2.19)  are  unmodified  if  the  client  first 
marginalizes  out  these  earlier  server  states. 

Recovery  Packet 

Passively  shifting  the  origin  limits  the  robustness  of  the  OSM  algorithm.  The  server  can  no  longer 
guarantee  that  the  client  has  received  the  origin  TOL  state  (or  the  previous  origin  state,  as  described 
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above).  If  the  client  vehicle  has  not  received  an  update  in  a  sufficiently  long  period  of  time,  it  will 
require  a  special  information  packet  in  order  to  recover  (i.e.,  reconstruct  the  current  server  pose- 
graph  given  the  state  it  has  already  received).  Once  the  client  identifies  that  it  has  lagged  behind,  it 
transmits  a  recovery  request  to  the  server  containing  the  last  received  TOL  index.  After  receiving 
a  client  request,  the  server  computes  this  special  information  as  an  additive  ‘delta  information’ 
(discussed  in  Section  2.3)  from  the  last  TOL  state  that  the  client  has  received  up  to  the  current 
origin  state.  One  implementation  detail  here  is  that  now  the  server  must  not  marginalize  out  the 
oldest  TOL  states  from  its  pose-graph  in  order  to  compute  a  recovery  packet,  unless  it  can  guarantee 
that  each  client  has  received  a  more  recent  TOL.  The  full  client-side  operation  is  summarized  in 
Algorithm  2. 

Recovery  requests  could  limit  the  scalability  of  the  algorithm  because  each  client  vehicle  re¬ 
quires  a  slot  in  the  TDMA  schedule  to  transmit.  In  our  field  trials  (Section  2.4),  recoveries  were 
rarely  necessary,  so  that  only  a  small  fraction  of  the  TDMA  was  reserved. 


2.3  Online  Distributed  Estimation 


We  demonstrate  that  the  incremental  reconstruction  of  the  server  pose-graph  within  the  OSM 
framework  can  be  used  onboard  the  client  to  exactly  reproduce  the  centralized  solution  to  the 
multiple  vehicle  localization  problem.  We  couple  the  OSM  algorithm  with  the  DEIF  algorithm 
update  (Webster  et  al.,  2013)  to  compute  the  client-side  state  estimate  following  OWTT  range 
observations.  The  DEIF  algorithm  is  a  method  in  which  a  client  vehicle  can  exactly  reproduce 
the  centralized  delayed-state  filter  solution  for  server-client  cooperative  networks.  Essentially,  the 
DEIF  provides  an  efficient  way  to  incorporate  the  newest  server  information  in  a  delayed-state 
framework.  The  DEIF,  as  originally  proposed,  is  not  real-time  practical  since  it  relies  on  an  unre¬ 
alistic  communication  assumption  (perfect  packet  reception)  to  build  the  server  information — this 
is  remedied  by  the  OSM  representation.  The  full  operation  of  the  OSM  and  DEIF  is  illustrated  in 
Fig.  2.1. 

To  review  the  DEIF,  the  server  vehicle  maintains  an  information  filter  to  fuse  its  local  mea¬ 
surements,  augmenting  its  state  vector  with  each  TOL  position.  As  with  the  OSM,  the  server  state 
evolves  as  a  Markov  process.  Each  ‘delta  information’  encompasses  all  the  local  information  that 
the  server  has  gained  between  TOLs,  computed  as 


^■A-Sn  —  A Sn  ASri_1, 
—  'n  sn  —  "n  s„_i> 


(2.21) 


where  the  operation  conforms  for  the  dimensionality  difference  and  the  ‘s’  subscript  indicates  the 
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Figure  2.7  Illustration  of  the  DEIF  update  step.  The  client  adds  the  server  delta  information  to  exactly  reconstruct  the 
centralized  information. 


aaS4  =  AS4  -  aS3 

(a)  Server-side  DEIF  operation.  Server  information  is  indicated  in  red. 


. 


Ac4  —  ^C4  +  AA-S4 

(b)  Client-side  DEIF  operation.  Local  client  information  and  range  information  are  shown  in  blue  and 
green,  respectively. 


server’s  information.  The  delta  information  is  illustrated  in  Fig.  2.4  and  Fig.  2.7.  Delta  information 
packets  can  be  conceptually  considered  as  expressing  a  transition  on  the  server  pose-graph  from 
the  previous  TOL  state  to  the  current  TOL  state.  The  DEIF  design  insightfully  noted  that  the  delta 
information  updates  computed  by  the  server  are  equivalent  to  the  delta  information  updates  within 
the  CEIF. 


The  client-side  DEIF  is  driven  by  its  local  measurement  updates  and  periodic  (assumed  non- 
lossy)  delta  information  packets  from  the  server  vehicle,  which  the  fault-tolerant  OSM  algorithm 
provides.  The  client-side  DEIF  tracks  the  current  client  state  in  addition  to  the  set  of  server  TOL 
states 


= 


T 


where  the  ‘c’  subscript  indicates  the  client  state.  Upon  packet  reception,  the  client  vehicle  simply 
adds  the  delta  information  into  its  information  filter 


Acn  =  A'n+AAS„, 


(2.22) 


where  information  includes  both  server  TOL  states  and  the  current  client  state.  Following  the 
subsequent  relative  range  measurement  update,  the  client-side  filter  matches  the  corresponding 
centralized  filter  exactly  up  to  communication  delay.  The  delta  information  addition  is  illustrated 
in  Fig.  2.7.  The  client  is  not  required  to  maintain  the  full  set  of  server  TOL  poses  in  its  state  vector. 
Full  details  of  the  algorithm  as  well  as  extensive  comparitve  results  are  provided  in  Webster  et  al. 
(2013). 

The  DEIF  requires  that  the  client  receive  each  delta  information  (2.21)  broadcast  by  the  server 
in  order  to  reconstruct  the  server  pose-graph.  We  use  the  OSM  to  reconstruct  the  server  pose-graph 
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onboard  the  client.  The  client  vehicle  is  then  able  compute  the  delta  information  to  the  DEIF,  and 
therefore  locally  reconstruct  the  CEIF. 

2.4  Field  Trials 

Seven  field  trials  spanning  more  than  12  h  of  operation  with  a  single  server  and  two  client  ve¬ 
hicles  were  performed  over  August  18-20,  2013,  at  the  University  of  Michigan  Biological  Sta¬ 
tion  (UMBS).  These  trials  demonstrate  the  OSM  algorithm’s  ability  to  incrementally  broadcast 
and  reconstruct  the  server  pose-graph,  compute  the  centralized  solution,  and  fuse  range-only  con¬ 
straints  in  a  multiple  vehicle  framework.  In  addition  to  the  single  server-client  topology,  we  provide 
post-process  results  demonstrating  a  two-server  support  network. 

2.4.1  Experimental  Setup 

We  fielded  two  Ocean-Server,  Inc.  Iver2  AUVs,  designated  AUV1  and  AUV2,  (Fig.  2.8).  Each 
AUV  is  outfitted  with  an  advanced  dead-reckoning  sensor  suite  outlined  in  Appendix  C.  Through¬ 
out  our  experiments,  AUV1  acts  as  the  server,  aiding  AUV2,  which  is  considered  to  be  the  client. 
AUV  1  is  the  only  vehicle  that  observes  and  fuses  GPS  when  at  the  surface.  To  demonstrate  the 
ability  of  our  OSM  algorithm  to  support  multiple  client  vehicles,  we  also  consider  a  topside  support 
ship  (with  only  GPS  reported  velocity  and  heading  for  input)  as  a  client  vehicle.  All  vehicles  were 
outfitted  with  a  Woods  Hole  Oceanographic  Institution  (WHOI)  Micro-modem  and  co-processor 
board  capable  of  encoding  multiple  frame,  higher  bandwidth  phase-shift  keying  (PSK)  data  packets 
(Freitag  et  al.,  2005a, b). 

For  baseline  comparision  of  our  OWTT  navigation,  we  deployed  a  three-beacon  25  kHz  FBF 
network  to  independently  measure  underwater  vehicle  position.  Fig.  2.9  depicts  the  FBF  beacon 
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Figure  2.9  Experiment  B  setup  with  three  LBL  beacon  locations. 


locations  used  during  our  field  trials,  which  were  positioned  to  provide  good  triangulation  observ¬ 
ability  over  the  client  vehicle  survey  area  (the  beacon  locations  were  moored  and  held  fixed  for  all 
experiments).  Each  vehicle  interrogated  the  LBL  network  roughly  twice  per  minute,  resulting  in 
a  maximum  of  six  range  constraints  per  minute.  We  recorded  two-way  LBL,  along  with  GPS  po¬ 
sition  fixes  at  the  surface,  for  all  vehicles  for  ground-truth  comparison  (none  of  the  client  vehicles 
used  these  measurements  during  the  real-time  experiments). 

2.4.2  Vehicle  State  Description 

Since  AUV  attitude  and  depth  are  both  instrumented  with  small  bounded  error,  we  focus  on  world- 
frame  XY  horizontal  position  estimation.  By  broadcasting  pressure  depth  with  each  acoustic 
packet,  OWTT  3D  range  measurements,  z3D,  can  be  projected  into  the  horizontal  plane,  zr,  ac¬ 
cording  to  the  method  outlined  in  Appendix  B.  Moreover,  we  are  motivated  to  maintain  a  minimal 
state  size  because  of  the  limited  acoustic  channel  capacity. 

The  state  estimator  on  each  vehicle  tracks  a  state  vector  composed  of  its  horizontal  position, 
x/c  =  \xk,yk]T .  The  state  is  time-propagated  using  an  odometry-driven  plant  model, 


x/c-j-i  X/,.  T 


where  is  the  delta  odometry  measurement.  The  odometry  input  and  corresponding  input 
covariance,  Q^+i,  are  obtained  by  Euler  integrating  DVL  and  attitude  heading  reference  sys¬ 
tem  (AHRS)  measurements  and  performing  a  first-order  covariance  estimate  as  described  in  Ap¬ 
pendix  B.  In  the  case  of  the  topside  surface  craft,  world-frame  velocity  is  integrated  from  GPS 
reported  speed  and  track  direction. 

Lor  the  server  vehicle,  GPS  reported  x,  y  observations  at  vehicle  surfacings  are  treated  as  linear 
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Figure  2.10  Acoustic  message  composition.  Each  PSK  Rate  1  and  Rate  2  Micro-modem  message  contains  three  64- 
byte  frames.  We  use  the  first  two  frames  to  hold  two  origin  state  packets  and  the  third  frame  is  reserved  for  recovery 
packets,  or  an  additional  origin  state  packet  if  no  recovery  has  been  requested. 


Primary  (n^ok)  Secondary  (ok^Oj)  Tertiary  (Oj<-Oi) 


f  Frame  1  'V  Frame  2  ^ 

Frame  3  A 

Origin  State  Packet  (60B)W 

'Origin  State  Packet  (60B))  , 

[OSP/Recovery  (60B)  ) 

< -  64B  - > 

< -  64B  - > 

\k -  64B  - > 

observations  of  state.  OWTT  measurements,  zr,  provide  a  range  between  the  server  TOL  position 
and  the  client  TOA  position,  with  nonlinear  observation  model: 


zr 


X«TOL  XCTOA 


+  v, 


where  v  ~  A/"(0,  a‘f)  represents  the  range  measurement  noise.  In  our  experiments,  we  used  ar  = 
30  cm,  to  account  for  noise  in  both  TOF  observation  and  depth. 


2.4.3  Acoustic  Communication  Considerations 

Each  origin  state  packet  requires  60  bytes  to  encode.  Each  double  precision  element  of  the  origin 
state  information  is  rounded  to  a  precision  of  10-5  to  reduce  the  packet  size.  Moreover,  since 
the  information  matrix  is  symmetric,  only  the  upper  diagonal  elements  are  transmitted.  The  full 
message  specification  is  detailed  in  Appendix  C.  Both  Micro-modem  PSK  Rate  1  and  Rate  2 
messages  allow  the  user  to  broadcast  three  64  byte  frames  (Fig.  2.10).  We  fill  the  first  two  frames 
of  Rate  1  and  Rate  2  messages  with  the  primary  and  secondary  OSPs  as  discussed  in  Section  2.2.2. 
If  a  client  vehicle  has  requested  a  recovery  packet,  we  transmit  the  custom  recovery  packet  in 
the  remaining  third  frame,  so  that  normal  operation  continues  for  vehicles  that  do  not  require 
a  recovery  step,  otherwise  we  broadcast  a  tertiary  OSP.  Acoustic  packets  were  encoded  using 
dynamic  compact  control  language  (DCCL)  (Schneider  and  Schmidt,  2010a)  and  transmitted  using 
the  Goby-acomms  library  (Schneider  and  Schmidt,  2012). 

We  employed  a  fixed  TDMA  cycle,  whereby  all  vehicles  were  assigned  a  communication  time 
slot.  The  server  vehicle  broadcast  an  OSP  roughly  once  per  minute,  while  the  client  transmitted 
a  single  data  packet  over  the  same  time  window,  used  to  monitor  vehicle  health  state  and  to  place 


Table  2.2  Acoustic  statistics  across  Experiments  A-G 


A 

B 

C 

D 

E 

F 

G 

Server AUV 1  broadcast  bps 

25.6 

25.1 

22.4 

23.0 

23.0 

22.8 

23.2 

Client:AUV2  Reception  rate 

47.9% 

43.2% 

32.9% 

32.1% 

53.0% 

34.4% 

33.7% 

Client:  Topside  Reception  rate 

46.1% 

55.9% 

62.6% 

57.1% 

60.2% 

50.4% 

39.9% 
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Figure  2.11  Pose-graph  reconstruction  example  from  Experiment  G.  Light  gray  trajectory  depicts  the  server  pose- 
graph  over  all  poses,  while  the  black  pose-graph  represents  server  TOL  states.  The  thick  red  line  represents  the 
client-side  reconstructed  server  pose-graph  (as  if  missed  server  TOL  poses  had  been  marginalized  out).  The  two 
pose-graphs  are  equal  up  to  communication  round-off  errors. 


recovery  requests  when  needed.  As  noted  in  Table  2.2,  the  average  server  throughput  used  for 
navigation  data  was  ~25  bps.  The  average  client-side  navigation  packet  reception  rates  across 
experiments  varied  between  32%  and  63%.  Our  origin  state  method  allowed  each  client  to  reliably 
reconstruct  the  server  pose-graph  despite  the  small  bandwidth  allotment  and  low  reception  rates. 

2.4.4  Results 

Fig.  2.12  summarizes  the  relative  vehicle  trajectories  over  the  seven  individual  field  trials.  The 
relative  server-client  geometries  between  AUV1  and  AUV2  were  purposely  varied  between  the 
different  experiments  while  the  topside  surface  craft  had  no  control  and  drifted  around  the  survey 
site,  occasionally  motoring  to  stay  within  the  site  boundary.  During  Experiments  A  and  B,  the 
server  and  client  swam  on  orthogonal  lawn-mower  trajectories,  Experiments  C  and  D,  the  server 
and  client  swam  along  the  same  lawn-mower  trajectory  with  the  server  following  at  a  fixed  dis¬ 
tance,  Experiment  E,  the  server  encircled  the  client  via  a  bounding  diamond- shaped  trajectory, 
while  during  Experiments  F  and  G,  the  server-client  swam  along  the  same  lawn-mower  trajectory, 
beginning  at  different  boundaries  of  the  survey  area.  As  seen  in  Table  2.2,  the  different  relative 
geometries  and  conditions  led  to  varied  communication  reception  performance  ranging  from  32- 
63%  throughput.  During  the  course  of  our  experiments,  each  vehicle  swam  at  fixed  depth  with 
AUV1  holding  a  depth  of  8  m,  AUV2  at  10  m  (apart  from  prescribed  surface  intervals  for  GPS 
ground-truth),  and  the  topside  transducer  was  suspended  at  10  m  depth. 
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Figure  2.12  Summary  of  the  seven  field  trials  used  for  experimental  evaluation,  (a)-(g)  Topdown  view  of  the  3-node 
vehicle  trajectories  with  total  trial  time  indicated  in  each  subcaption.  AUV2  and  Topside  acted  as  clients  while  AUV1 
performed  the  server  role,  (a)  AUV1  shifted  the  origin  forward  at  an  accelerated  rate  in  order  to  artificially  induce 
recovery  requests,  (b)  AUV1  and  AUV2  followed  orthogonal  lawnmower  surveys.  (c),(d)  AUV1  followed  AUV2 
along  the  same  lawnmower  survey,  (e)  AUV1  maintained  a  diamond  box  path  bounding  AUV2’s  lawnmower  survey. 
(f),(g)  AUV1  and  AUV2  followed  similar  lawnmower  surveys,  beginning  from  opposite  ends  of  the  survey  area,  i.e., 
AUV 1  began  on  the  East  boundary  while  AUV2  began  on  the  West  boundary. 


(a)  Experiment  A  (0.94  h) 


(b)  Experiment  B  (2.02  h) 


(c)  Experiment  C  (1.87  h) 


-400  -300  -200  -100  0  100  200  300 

East  [m] 


-300  -200  -100  0  100  200  300 

East  [m] 


(d)  Experiment  D  (1.92  h) 


(e)  Experiment  E  (2.01  h) 


(f)  Experiment  F  (1.63  h) 


(g)  Experiment  G  (1.65  h) 


Client-side  Server  Pose-graph  Reconstruction 

The  client  was  able  to  accurately  reconstruct  the  server  pose-graph  using  the  OSM  framework 
throughout  all  of  our  field  trials.  Fig.  2.1 1  illustrates  an  example  ‘true’  server  pose-graph  with  the 
client-side  reconstruction  overlaid.  Note  that  the  client-side  reconstruction  does  not  contain  all  of 
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Figure  2.13  Server  OSP  TOL  indices  and  the  corresponding  index  received  by  the  client  during  Experiment  D.  The 
shaded  red/green/blue  regions  represent  the  OSPs  broadcast  by  the  server  (color  coded  according  to  Fig.  2.10),  while 
the  thick  black  line  plots  the  client’s  latest  received  server  index.  Large  steps  in  the  server  indices  indicate  an  origin 
shift,  (a)  illustrates  the  real-time  server  origin  shifting,  while  (b)  demonstrates  how  the  OSM  algorithm  adapts  to 
varying  conditions,  waiting  longer  before  shifting  the  origin  when  GPS  is  cut  out.  Note  that  the  real-time  server 
shifted  the  origin  four  times  during  the  same  region  whereas  the  post-process  origin  only  shifted  twice  (indicated  by 
dashed  lines). 


mission  time  [s]  mission  time  [s] 


(a)  GPS  available  during  surface  intervals  (b)  No  GPS  during  shaded  region 


the  server  TOL  poses.  The  client’s  version  of  the  server  pose-graph,  however,  is  equivalent  to  the 
server’s  as  if  the  TOL  states  corresponding  to  dropped  messages  had  been  marginalized  out.  As 
shown  in  Fig.  2.14a,  each  client  is  able  to  reconstruct  the  server  pose-graph  with  small  error,  on 
average  to  the  order  of  10“5  m.  Moreover,  this  error  is  attributed  to  the  communication  round-off 
of  the  OSP  (using  the  origin- shifting  scheme  with  full  precision  OSPs  in  post-process,  both  mean 
and  max  reconstruction  error  is  on  the  order  of  10“ 12  m). 

Server  Origin  Shifting 

During  Experiment  A,  the  server  shifted  the  origin  forward  at  an  artificially  increased  rate  (every 
other  TOL)  in  order  to  force  a  client  recovery  request.  During  this  trial,  AUV2  and  Topside  received 
one  and  three  recovery  packets,  respectively,  after  transmitting  requests  to  the  server.  These  packets 
were  successfully  integrated  into  the  server  pose-graph  reconstruction. 

Throughout  the  remaining  11  h  of  field  trials  showcasing  normal  operation,  neither  client  re¬ 
quired  a  recovery  packet.  Moreover,  during  normal  operation  (Experiments  B-G),  both  clients 
used  a  total  of  554  Frame  1  OSPs,  62  Frame  2  OSPs,  and  only  2  Frame  3  OSPs.  The  server  used  a 
trace  threshold  T  =  5  •  10“2. 

The  client  will  require  a  secondary  (Frame  2)  OSP  at  most  once  per  server  origin  shift,  because 
the  secondary  packet  ’catches’  the  client  up  to  the  current  origin.  We  expect  the  client  to  occasion¬ 
ally  require  a  secondary  packet  following  an  origin  shift  because  of  the  high  likelihood  of  missing 
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any  single  transmission.  However,  the  client  would  need  to  lose  communication  with  the  server 
over  an  entire  period  between  origin  shifts  in  order  to  require  the  tertiary  (Frame  3)  OSP.  In  order 
to  require  a  recovery  packet,  communication  would  have  to  drop  out  over  at  least  two  origin  shift 
periods,  depending  on  the  number  of  broadcast  redundant  OSPs. 

The  rate  at  which  the  server  will  shift  the  origin  (based  on  the  trace  metric  in  (2.20))  depends 
on  the  server  noise  models  and  available  measurements.  Absolute  position  observations,  e.g.,  GPS, 
and  noisy  process  models  will  reduce  correlation  between  the  current  state  and  the  origin  state,  so 
that  subsequent  measurements  will  not  influence  the  origin  as  much.  Therefore,  after  receiving 
absolute  position  observations  or  sufficient  time  given  a  noisy  process  model,  the  server  will  be 
forced  to  shift  the  origin.  Fig.  2.13  illustrates  the  server’s  origin  shifting  during  Experiment  D. 
The  server  surfaced  at  regular  intervals,  receiving  several  GPS  observations.  In  post-process,  we 
cut  out  GPS  measurements  over  a  nearly  30  min  window.  In  this  case,  the  server  shifted  the  origin 
forward  less  frequently  (twice  as  opposed  to  four  times  as  seen  in  Fig.  2.13b).  This  demonstrates 
the  ability  of  the  OSM  algorithm  to  automatically  adapt  origin  shifting  to  varying  measurement 
availability. 

Client-side  DEIF 

Each  client  employed  a  DEIF  to  integrate  server  and  client  information  with  relative  range  observa¬ 
tions.  The  state  estimate  of  the  post-process  CEIF  agrees  with  our  real-time  DEIF  with  differences 
commensurate  with  those  reported  by  Webster  et  al.  (2013)  (on  the  order  of  10”5  m)  and  on  par 
with  the  errors  observed  in  the  server  pose-graph  reconstruction  (see  Fig.  2.14b).  The  two  esti¬ 
mates  are  equal  at  the  TOA  of  each  OSP.  The  estimates  may  vary  in  between  TOAs  because  the 
CEIF  is  able  to  incorporate  server  information  the  instant  it  is  received,  while  the  DEIF  must  wait 
to  incorporate  server  information  until  the  OSP  is  received  (i.e.,  up  to  communication  delay). 

Multiple  Server  Implementation 

It  is  a  relatively  simple  extension  to  move  from  a  single  to  multiple  independent  server  implemen¬ 
tation  as  is  depicted  in  Fig.  2.3.  In  this  case,  the  client  vehicle  reconstructs  the  local  pose-graph 
from  each  server  and  fuses  relative  range  constraints.  Range  constraints  are  still  measured  by  the 
client  to  each  server.  The  client-side  DEIF  reproduces  the  centralized  filter  (up  to  communication 
delay).  Communication  delay  will  cause  the  linearization  point  used  for  range  observations  to  dif¬ 
fer  between  the  client-based  and  centralized  results.  This  is  because  one  (or  both)  of  the  servers 
will  have  made  observations  that  the  client  has  not  yet  received. 

We  tested  the  two  server  scenario  in  post-process  using  the  field  collected  sensor  data  from 
Experiment  C.  In  this  trial,  the  Topside  vehicle  acted  as  the  client  with  AUV1  and  AUV2  acting 
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Figure  2.14  Summarized  reconstruction  and  estimation  error  across  all  seven  field  trials  (A-G).  (a)  The  server  pose- 
graph  reconstruction  error  is  computed  as  the  norm  of  the  difference  between  server  TOL  poses  received  on  the  client 
and  their  actual  value  computed  on  the  server.  The  maximum  error  for  any  single  pose  remains  below  1  mm  for  all 
trials,  while  the  mean  error  is  on  the  order  10-5  m.  (b)  Difference  in  the  CEIF  versus  the  DEIF  client  trajectory 
estimate  at  the  TOA.  Each  bar  represents  the  mean  norm  difference  between  state  estimates  for  each  estimator.  Note 
that  the  client-side  DEIF  is  able  to  reproduce  the  centralized  CEIF  estimate  in  real-time  to  high  accuracy. 


Experiment  label 


(a)  Client-side  server  pose-graph  OSM  reconstruction  error 


10  4  m  error 


g  0.00010 


Experiment  label 

(b)  Client-side  decentralized  estimate  (DEIF)  versus  centralized  benchmark 
(CEIF) 


as  servers.  The  Topside -based  DEIF  solution  differs  with  the  corresponding  CEIF  on  average  by 
4.5  cm.  We  can  attribute  this  difference  primarily  to  a  difference  in  range  observation  linearization 
point. 

Performance  Baseline 

OWTT  navigation  allows  AUVs  to  navigate  subsea  for  extended  periods  of  time  with  bounded 
error.  Bounded  error  navigation  is  usually  achieved  with  the  use  of  networks  of  stationary  acous¬ 
tic  beacons,  e.g.,  LBL  or  USBL.  The  cost  of  deploying,  surveying,  and  later  recovering  an  LBL 
system  is  high  with  respect  to  time,  however.  Here,  we  discuss  the  ability  of  OWTT  relative  rang¬ 
ing  frameworks  to  approach  the  accuracy  of  LBL  systems  without  the  operational  and  equipment 
overhead.  The  accuracy  of  the  post-process  CEIF  has  previously  been  extensively  compared  to  an 
LBL  navigation  solution  by  Webster  et  al.  (2012). 

Fig.  2.15  compares  both  LBL  and  OWTT  based  DEIF  navigation  solutions  to  a  baseline  tra¬ 
jectory  computed  by  fusing  GPS  and  odometry  measurements.  Since  each  range  observation  only 
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Figure  2.15  OWTT  and  LBL  navigation  comparison  to  a  GPS  baseline.  OWTT  compares  well  with  LBL  in  (a), 
while  OWTT  has  a  larger  error  in  (b)  (outlined  in  red  in  the  lower  right  corner).  The  server/client  relative  geometries 
(Fig.  2. 12),  however,  greatly  influence  this  result.  The  middle  axis  in  each  subfigure  plots  the  x,  y  uncertainty  estimated 
by  the  DEIF,  showing  that  in  (b)  y,  East,  uncertainty  is  not  as  well  bounded.  Finally,  the  lower  axis  shows  the  norm 
difference  between  OWTT  and  LBL  solutions. 


mission  time  [s] 


mission  time  [s] 


(a)  Experiment  B:  good  server-client  geometry  (b)  Experiment  C:  poor  server-client  geometry 


adds  information  in  a  single  direction,  the  relative  geometry  between  server  and  client  is  of  ut¬ 
most  importance.  Experiment  B  (Fig.  2.15a)  intuitively  has  an  informative  relative  geometry  as 
the  server  continually  crosses  over  the  clients  path,  helping  to  bound  error  in  both  x,  y  directions. 
Experiment  C  (Fig.  2.15b),  however,  does  not  have  such  a  useful  relative  geometry,  as  the  server 
largely  remains  behind  the  client  along  the  North-South  direction.  Indeed,  we  see  that  the  server  is 
able  to  well  bound  uncertainty  along  the  East-West  direction  during  Experiment  B,  but  not  as  well 
in  Experiment  C.  Moreover,  due  to  an  informative  relative  server  trajectory,  the  client  DEIF  closely 
reproduces  the  full  LBL  solution  in  Experiment  B.  This  is  further  evidenced  by  the  norm  differ¬ 
ence  between  LBL  and  DEIF  solutions  (Fig.  2.15),  which  illustrates  that  the  difference  between 
solutions  decreases  in  Experiment  B  but  increases  with  mission  time  in  Experiment  C. 

Each  client  vehicle  received  at  most  one  OWTT  relative  range  constraint  over  each  one  minute 
TDMA  window,  while  potentially  receiving  six  LBL  ranges  over  the  same  period.  Using  OWTT 
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navigation,  however,  we  can  achieve  bounded  error  on  the  same  order  as  LBL.  This  solution  is 
only  enhanced  by  selecting  appropriate  server  relative  geometries.  Advantageously,  our  method 
also  scales  to  many  client  vehicles,  whereas  the  update  rate  for  narrowband  LBL  decreases  with 
new  clients.  Optimal  relative  positioning  of  server  vehicles  has  received  recent  attention  (Tan  et  al., 
2014;  Bahr  et  al.,  2012),  and  is  the  subject  of  Chapter  4. 

2.5  Discussion 

The  OSM  with  the  DEIF  constitutes  a  smoothing  algorithm  built  upon  a  delayed-state  information 
filter.  While  an  extended  information  filter  is  sub-optimal  for  nonlinear  estimation,  our  solution 
compares  well  to  nonlinear  smoothing  algorithms  such  as  iSAM  (Kaess  et  al.,  2008).  The  delayed- 
state  information  filter  is  equivalent  to  a  nonlinear  least-squares  approach  with  the  exception  that 
measurement  constraints  are  only  linearized  once  upon  their  initialization.  This  difference  is  small 
in  our  implementation  as  all  odometry  constraints  are  linear,  and  infrequent  range  measurements 
account  for  the  only  nonlinearities. 

The  OSM  algorithm  reproduces  the  linear  server  information  up  to  communication  round-off 
errors  onboard  the  client.  The  delta  information  (2.21)  computed  from  the  client-side  reconstructed 
pose-graph  is  a  linear  factor  over  the  previously  received  and  current  server  TOL  states.  This 
factor  contains  all  information  due  to  observations  occurring  between  the  TOLs  including  process 
predictions,  odometry,  and  absolute  position  measurements  such  as  GPS.  Fig.  2.16,  for  example, 
illustrates  a  delta  factor  as  summarizing  all  information  that  occurs  between  server  TOF  poses. 

We  can  compute  a  factor  to  represent  the  delta  information  using  the  concept  of  generic  linear 
constraints  (GFCs)  proposed  by  Carlevaris-Bianco  and  Eustice  (2013).  First,  we  compute  the 


Figure  2.16  Server  factor  graph  example.  The  above  factor  graph  shows  the  full  server  factor  graph.  Shaded  nodes 
and  edges  indicate  constraints  occurring  between  the  previous  and  current  TOL.  The  lower  graph  illustrates  the 
reconstructed  client-side  factor  graph.  The  shaded  delta  state  factor  induces  an  equivalent  factor  potential  as  the 
original  server-side  factors. 
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Figure  2.17  Difference  in  estimated  pose  between  the  DEIF  algorithm  and  the  nonlinear  least-squares  iSAM  during 
Experiment  F.  The  mean  and  maximum  differences  are  0.079  m  and  0.35  m,  respectively. 


eigen-decomposition  of  the  delta  information 

AA  =  UDUt, 

where  AA  G  Mpxp,  q  =  rank(AA),  U  G  W,xq,  and  D  G  M9X<?  is  a  diagonal  matrix.  Using  the 
decomposition,  we  define  the  delta  factor  as  a  Gaussian  observation  with  mean  and  covariance 

za  =  D2UTtc  (2.23) 

Sa  =  I<jxg5  (2.24) 

where  x  =  [x^_  ,  xJJT  is  the  state  vector  over  the  previous  and  current  server  TOL  poses.  This 
factorization  (while  not  unique)  produces  the  same  information  over  the  server  TOL  poses  as  the 
additive  delta  information  block. 

iSAM  is  equivalent  to  the  DEIF  with  the  exception  that  measurement  constraints  may  be  re¬ 
linearized  after  their  initialization.  This  difference  is  small  in  our  implementation  as  all  odometry 
constraints  are  linear,  and  infrequent  range  measurements  account  for  the  only  nonlinearities. 

We  implemented  the  factor  representation  for  delta  information,  (2.23)  and  (2.24),  to  provide 
server  information  to  the  client.  Fig.  2.17  plots  the  difference  between  the  DEIF  and  iSAM  for 
Experiment  F.  The  small  difference  in  DEIF  and  iSAM  estimates  is  due  to  differing  lineariza¬ 
tion  points.  The  ability  to  relinearize  range  constraints  may  have  additional  benefits  over  the 
DEIF  when  a  good  linearization  point  is  initially  not  available,  for  example,  when  the  client  dead- 
reckoned  error  has  grown  too  large. 
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2.6  Chapter  Summary 


This  chapter  presents  the  OSM  for  server-client  cooperative  localization  over  an  extremely  faulty 
and  bandwidth-limited  communication  channel.  Moreover,  the  OSM  algorithm  allows  a  client 
vehicle  to  exactly  reproduce  the  centralized  estimate — therefore  guaranteeing  that  the  estimate  is 
consistent.  We  validated  this  distributed  estimation  algorithm  over  more  than  12  h  of  field  trials 
deploying  three  vehicles.  We  demonstrated  how  our  algorithm  is  adaptable  to  support  both  multiple 
clients  and  multiple  servers,  albeit  while  maintaining  strict  server-client  communication. 
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Chapter  3 

Peer-to-peer  Localization 


In  the  previous  chapter,  we  presented  a  method  for  server-client  cooperative  localization  that  ex¬ 
ploited  the  structure  of  the  underlying  Gaussian  MRF  in  order  to  distribute  inference.  The  OSM 
algorithm  allowed  the  client  vehicle  to  reconstruct  exactly  the  corresponding  centralized  estima¬ 
tor.  However,  OSM  requires  an  unwieldy  recovery  mechanism  because  the  server  pose-graph 
reconstruction  is  limited  by  machine  numerical  precision.  Moreover,  OSM  requires  that  the  server 
distribution  grows  according  to  a  Markov  chain.  In  this  chapter,  we  present  an  algorithm  that  is 
similar  in  spirit,  but  exploits  the  structure  of  the  underlying  factor  graph1 .  By  exploiting  properties 
of  the  constituent  factor  types,  we  derive  an  algorithm  that  is  completely  tolerant  to  communication 
failure  (i.e.,  never  requires  a  recovery  packet)  and  is  extensible  to  other  graph-based  factors.  Each 
vehicle  reconstructs  a  consistent  approximation  to  the  full  centralized  estimator.  This  method, 
while  approximate,  more  readily  scales  to  larger  vehicle  networks  toward  peer-to-peer  cooperative 
localization. 

We  employ  approximate  marginalization  techniques  to  enable  vehicles  to  share  an  informative 
subset  of  composed  factors.  Approximate  marginalization  has  recently  become  a  popular  tool 
within  the  SLAM  community  for  reducing  graph  complexity,  e.g.,  (Vial  et  al.,  2011;  Carlevaris- 
Bianco  et  al.,  2014;  Huang  et  al.,  2013;  Mazuran  et  al.,  2014).  In  this  work,  we  exploit  similar 
tools  in  order  to  efficiently  distribute  locally  obtained  information  within  a  cooperative  localization 
framework. 

The  specific  contributions  of  this  chapter  include: 

•  We  present  a  cooperative  localization  algorithm  for  communication-limited  networks  that  is 
completely  passive  (i.e.,  does  not  rely  on  acknowledgments)  and  employs  a  fixed  bandwidth 
data  packet. 

•  We  approximate  the  true  locally  constructed  factor  graph  using  approximate  marginalization 
'Portions  of  this  work  appear  in  (Walls  et  al.,  2015b). 
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techniques  to  produce  a  structure  that  can  be  more  easily  broadcast. 

•  We  provide  a  comparative  evaluation  of  our  algorithm  and  a  performance  summary  from  a 
three  vehicle  field  deployment  including  two  AUVs  and  a  topside  ship. 


3.1  Related  Work 

Within  cooperative  localization  frameworks,  teams  of  communicating  vehicles  localize  each  other 
by  sharing  local  sensor  data  and  observing  relative  vehicle  pose.  Early  work  explored  sharing 
information  among  the  team  and  performing  local  data  fusion  (Kurazume  et  al.,  1994;  Fox  et  al., 
2000;  Roumeliotis  and  Bekey,  2002;  Howard  et  al.,  2002;  Dellaert  et  al.,  2003). 

More  recent  methods  proposed  framing  cooperative  localization  as  a  multiple  vehicle  graph- 
based  SLAM  problem  (Kim  et  al.,  2010;  Cunningham  et  al.,  2013).  These  methods  implicitly 
handle  correlation  that  develops  between  vehicle  estimates  when  relative  information  is  shared; 
our  proposed  method  falls  within  this  category.  The  centralized  estimator  for  OWTT  coopera¬ 
tive  localization  shares  a  similar  structure  to  a  pose-graph  SLAM  formulation  with  known  data 
association. 

Leung  et  al.  (2010)  presented  a  decentralized  cooperative  localization  algorithm  capable  of  re¬ 
producing  the  centralized  estimate  in  the  presence  of  dropped  communication.  The  bandwidth  re¬ 
quired  for  transmitting  knowledge  sets,  however,  may  exceed  the  acoustic  channel  capacity  during 
periods  of  lost  communication  connectivity.  Nerurkar  and  Roumeliotis  (2013)  similarly  targeted 
cooperative  localization  in  bandwidth  constrained  scenarios,  but  require  constant  communication. 

Many  other  researchers  in  the  underwater  domain  have  considered  fusing  relative  range  mea¬ 
surements  derived  from  observing  the  TOF  of  acoustic  broadcasts.  Previous  work  (Vaganay  et  al., 
2004;  Maczka  et  al.,  2007;  Bahr  et  al.,  2009a,b;  Fallon  et  al.,  2010a;  Eustice  et  al.,  2011;  Webster 
et  al.,  2012,  2013;  Walls  and  Eustice,  2014a;  Pauli  et  al.,  2014)  has  considered  the  limitations  of 
the  acoustic  channel  and  the  difficulties  of  fusing  range-only  observations. 

The  complexity  of  graph-based  estimation  is  largely  dependent  on  the  size  (i.e.,  number  of 
variable  nodes)  and  sparsity  of  the  graph  (i.e.,  number  of  edges).  Graph  sparsification  methods 
have  been  introduced  (Vial  et  al.,  2011;  Carlevaris-Bianco  et  al.,  2014;  Huang  et  al.,  2013;  Mazuran 
et  al.,  2014)  to  both  reduce  the  number  of  variables  in  the  graph  and  increase  the  sparsity  without 
greatly  affecting  the  solution.  We  take  advantage  of  approximate  marginalization  techniques — 
used  in  graph  sparsification  to  reduce  the  number  of  variables — to  refactor  a  local  factor  graph 
over  a  smaller  set  of  pose  variable  nodes  to  reduce  the  communication  requirement. 

The  work  proposed  here  is  closest  to  Fallon  et  al.  (2010a)  and  the  OSM  presented  in  Chapter  2. 
Within  OSM,  we  exploited  the  structure  of  the  information  matrix  associated  with  the  underyling 
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Figure  3.1  Example  cooperative  localization  factor  graph — empty  circles  represent  variable  pose  nodes,  solid  dots 
are  odometry  and  prior  factors,  and  arrows  illustrate  range-only  factors  and  the  direction  of  communication.  In  this 
example,  red  represents  a  topside  ship  with  access  only  to  GPS,  while  blue  and  orange  represent  AUVs.  Take  note  that 
the  dashed  blue  factors  represent  a  new  set  of  approximate  factors  distinct  from  the  original  in  (a). 


(a)  Centralized  factor  graph  over  vehicle  network. 


A 


0  =  b 


(b)  Factor  graph  over  vehicle  network  constructed  onboard  the  orange  vehicle  by  our  algo¬ 
rithm. 


Gaussian  MRF  to  cope  with  unpredictable  packet  loss.  Unfortunately,  the  algorithm  requires  pe¬ 
riodically  shifting  an  origin  pose,  requires  a  recovery  mechanism  for  pathological  communication 
failures,  and  does  not  seamlessly  allow  for  a  vehicle  with  no  odometry  (e.g.,  a  surface  vehicle  with 
only  GPS)  to  participate  because  of  its  Markov  chain  assumption.  Fallon  et  al.  (2010a)  proposed 
constructing  a  graph-based  estimator  on  each  vehicle  by  broadcasting  individual  factors  to  the 
team.  Their  method  transmits  each  factor  over  a  large  set  of  pose  nodes  and  requires  data  requests 
when  communication  is  lost.  In  contrast,  we  exploit  the  factor  structure  of  the  local  graph  to  com¬ 
municate  information  within  a  completely  passive  framework.  Pauli  et  al.  (2014)  independently 
arrived  at  a  similar  solution  discussed  in  Section  3.4  and  Section  3.5. 


56 


3.2  Problem  Statement 


We  formulate  cooperative  localization  within  a  factor  graph  framework  (discussed  in  Appendix  A). 
We  show  that  the  underlying  structure  of  the  centralized  factor  graph  consists  of  factors  that  rep¬ 
resent  local  vehicle  observations  and  factors  that  represent  relative  vehicle  measurements.  We  will 
first  review  the  factor  graph  formulation  for  a  single  vehicle  and  then  expand  to  the  full  vehicle 
network. 

In  the  single  vehicle  setting,  the  factor  graph  approach  is  a  smoothing  algorithm  that  estimates 
the  entire  trajectory  of  the  vehicle.  Recall  that  a  factor  graph  is  a  bipartite  graph  with  pose  (variable) 
nodes  and  factor  (measurement)  nodes  representing  the  joint  distribution  over  the  unknown  poses. 
The  z th  vehicle  graph  represents  the  joint  distribution  over  its  N  poses,  X,  =  [x1? . . . ,  xjV],  as 

p(Xj|Zj)  oc  p(xi)  JJp(z0d0i|xj,Xj_i)  JJp(zprior.|xj),  (3.1) 

*  j 

where  Zt  represents  the  set  of  observations  and  we  assume  each  vehicle  has  access  to  its  initial 
belief  p(xi ).  The  graph  structure  is  a  chain  as  we  only  consider  unary  ‘prior’  factors,  zpn0r,  (e.g., 
GPS)  and  pairwise  sequential  ‘odometry’  factors,  zodo,  (e.g.,  integrated  velocity).  We  assume 
that  pose  variable  nodes  represent  rigid-body  position  and  orientation.  An  odometry  factor  is  the 
observed  rigid-body  transformation  between  two  poses. 

For  convenience,  we  define  a  ‘link’,  Ct  =  (zodo.,  zprior!),  associated  with  the  /th  pose  node,  x,, 
as  a  2-tuple  containing  the  odometry  factor  to  the  previous  pose  node  and  a  prior  factor.  Note  that 
each  link  need  not  have  both  an  odometry  and  prior  factor,  for  example  the  initial  link  only  contains 
a  prior  factor.  The  local  chain  is  the  set  of  links  that  represent  the  vehicle  trajectory  corresponding 
to  (3.1),  Ciocai  =  {£,  | .  In  Fig.  3.1a,  each  uniformly  colored  subgraph  represents  a  local  chain. 

We  can  construct  the  posterior  distribution  over  the  entire  M  vehicle  network  (i.e.,  all  vehicle 
poses),  {Xi,. . .  ,XM}, 

M 

p(X  1,  ....  XM|Zi,  .  .  .  ,  Z Mi  Zr)  oc  n^n  P(zk\xjk,Xjk\,  (3.2) 

1  ^locai^  ^  relative  factors 

where  Zr  is  the  set  of  relative  vehicle  constraints  and  each  z/,.  represents  a  constraint  between 
poses  on  vehicles  4  and  jk ■  In  this  chapter,  z/,  is  a  OWTT  range  constraint  between  a  transmitting 
vehicle’s  TOL  pose  and  a  receiving  vehicle’s  TOA  pose.  The  factor  graph  for  a  three  vehicle 
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network  is  illustrated  in  Fig.  3.1a.  The  centralized  MAP  estimate  for  each  vehicle  is  obtained 

X*  =  argnrax  p(X1? ....  XM|Zi, ....  ZM,  Zr) 

{Xi,...,XM} 

=  argmin  -  logp(Xi, . . . ,  XM|Z1; . . . ,  ZM,  Zr),  (3.3) 

{Xi,...,XM} 

which  results  in  a  nonlinear  least-squares  problem  for  Gaussian  noise  models  (Dellaert  and  Kaess, 
2006). 

The  full  joint  distribution  (3.2)  can  be  factored  as  a  product  of  each  vehicle’s  local  chain  and  the 
relative  vehicle  factors.  Therefore,  in  order  to  construct  (and  perform  inference  on)  the  full  factor 
graph,  the  ith  vehicle  must  have  access  to  the  set  of  local  factors  from  all  other  vehicles,  {C\oca\3  }j^i, 
and  the  set  of  all  relative  vehicle  factors.  Sharing  this  information,  however,  is  nontrivial  due  to 
the  limitations  of  the  acoustic  communication  channel. 

In  this  chapter,  we  present  an  algorithm  for  each  vehicle  to  communicate  an  approximation  of 
its  local  chain.  In  turn,  each  vehicle  in  the  network  can  construct  a  graph  consisting  of  its  local 
chain,  the  set  of  approximate  chains  received  from  other  vehicles,  and  the  set  of  relative  vehicle 
measurements  that  it  observes  locally,  i.e.,  range  constraints  for  which  it  has  measured  the  OWTT 
(a  subset  of  all  ranges)  as  in  Fig.  3.1b.  We  will  show  that  our  approximation  produces  an  accurate 
representation  that  allows  communication  over  a  faulty  and  bandwidth-limited  channel,  and  that 
using  a  subset  of  relative  vehicle  measurements  still  provides  significant  improvement  over  dead- 
reckoned  navigation. 

3.3  Cooperative  Localization 

The  key  requirement  for  cooperative  localization  is  the  ability  to  share  locally  obtained  observa¬ 
tions,  contained  here  within  a  local  chain.  Below,  we  develop  an  algorithm  for  broadcasting  a 
local  chain  across  a  faulty  low-bandwidth  communication  channel.  Our  approach  is  completely 
passive,  in  other  words,  the  information  each  vehicle  broadcasts  is  independent  of  the  rest  of  the 
network — a  desirable  property  for  an  unacknowledged  channel.  In  this  section,  we  outline  an  effec¬ 
tive  strategy  to  (i)  share  a  local  chain  (Algorithm  3)  and  (ii)  use  the  set  of  received  local  chains  and 
observed  relative  range  observations  to  compute  a  navigation  estimate  (Algorithm  4),  as  illustrated 
in  Fig.  3.1. 

A  local  chain  consists  of  odometry  and  prior  factors.  We  first  show  that  we  can  leverage  the 
composition  operation  over  odometry  factors  to  represent  each  new  odometry  factor  as  a  trans¬ 
formation  relative  to  an  ‘origin’  (Section  3.3.1).  A  corresponding  decomposition  operator  is  per¬ 
formed  onboard  the  receiving  vehicle  in  order  to  recover  the  original  factor.  Moreover,  both  of 
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Algorithm  3  Local  chain  distribution. 

1:  initialize(Ciocai,  Capprox)  {Prior  factor  on  initial  pose.} 

2:  while  is_running()  do 

3:  add_new_link(Ciocai)  {New  odometry  and  prior  factors.} 

4:  iff  is  TOL  then 

5:  append.appr ox (Ciocal,  Capprox)  {Section  3.3.2.} 

6:  L k  =  choose_links(CapProx)  {Section  3.3.3.} 

7:  \IK  =  compose_links(L^)  {Section  3.3.1.} 

8:  broadcast_links(L}^) 

9:  end  if 

10:  end  while 


these  operations  are  tolerant  to  communication  failure,  so  that  every  received  odometry  factor  can 
be  used  to  reconstruct  a  portion  of  the  local  chain. 

We  compose  the  odometry  factor  of  each  link,  £t  ,  with  all  previous  odometry  factors  to  obtain 
the  broadcast  link,  £'.  A  receiving  vehicle  reconstructs  the  transmitter’s  chain  consisting  of  de¬ 
composed  odometry  factors  and  prior  factors  contained  in  received  links.  If  a  broadcast  is  missed, 
upon  the  next  successful  reception  the  receiving  vehicle  can  still  reconstruct  the  chain  without  the 
prior  factor  in  the  missed  link.  We  rebroadcast  links  with  prior  factors  to  help  ensure  an  accurate 
chain  reconstruction,  meaning  that  at  each  TOL  we  broadcast  a  set  of  links,  L. 

Since  we  only  use  the  set  of  locally  observed  range  measurements,  each  vehicle  need  only 
share  TOL  pose  nodes.  However,  a  local  chain,  Clocal,  consists  of  many  additional  pose  nodes. 
To  reduce  the  communication  burden,  we  employ  approximate  marginalization  to  compute  an 
approximate  local  chain  CapProx  that  only  contains  odometry  and  prior  factors  over  TOL  pose  nodes 
(Section  3.3.2). 

The  ith  receiving  vehicle  is  able  to  reconstruct  the  broadcast  local  chains  by  decomposing 
broadcast  odometry  factors  with  odometry  factors  already  received.  Using  the  set  of  reconstructed 
approximate  local  chains  {Capprox.}.#**  its  own  local  chain  C|0ca|(.,  and  the  set  of  observed  OWTT 
range  constraints,  the  vehicle  can  construct  and  solve  a  batch  nonlinear  least-squares  problem  (3.3) 
to  estimate  its  smoothed  trajectory. 

3.3.1  Odometry  Composition 

In  this  section,  we  show  that  composition  of  rigid-body  transformations  is  reversible  (decomposi¬ 
tion).  Moreover,  it  is  equivalent  to  marginalization  over  the  odometry  chain — that  is,  the  marginal 
information  induced  by  the  the  full  set  of  odometry  factors  is  equal  to  the  information  induced  by 
the  composed  odometry  factor.  We  leverage  this  property  to  broadcast  each  link  Ci  E  Cappmx  over 
an  unreliable  communication  channel. 
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Composed  Odometry  Factor 

Rigid-body  transformation  (e.g.,  odometry)  observations  are  full  rank  relative-pose  constraints 
expressed  as 


z ij  =  hi:j(xi,  xj)  +  Wjj  (3.4) 

=  ©Xj  ©  xj  +  w ij,  (3.5) 

where  x*  and  x3  are  two  poses  with  respect  to  the  same  reference  frame,  wtJ  ~  A/"(0,  R,y)  is  an 
independent  additive  noise  vector,  and  ©  and  ©  represent  generalized  compounding  and  inverse 
operators.  These  operators  are  simple  addition  and  subtraction  for  poses  in  W1  and  are  nonlinear 
functions  defined  for  poses  on  SE( 2)  and  SE( 3)  (Smith  et  al.,  1986). 

Per  Smith  et  al.  (1986),  a  composite  observation  zlk  is  computed  through  the  sequence 


Z ik  di^iji  Zjk)  (3.6) 

=  z ^  0  z jk.  (3.7) 

The  composite  relation  zik  is  a  random  variable.  A  first-order  covariance  approximation  is  com¬ 
puted  through 


2 ik  ~  9 \Zij,zjk  Jl ®^zij  +  (3.8) 

~Af(  z ij  ©  z jk,  JieRyJi®  +  J2®  ) ,  (3.9) 

where  J  =  dg/d( zij}  zjk)  =  [Ji®,  J2©]- 

Suppose  we  are  given  the  composed  observations  (z^-,  R); )  and  (zlk.  R ik).  It  follows  directly 
from  (3.9)  that  we  can  invert  this  operation  to  recover  the  transformation  between  xj  and  x/,  and 


Figure  3.2  A  sequence  of  odometry  factors  can  be  composed  into  a  single  factor.  Likewise,  a  composed  factor  can  be 
decomposed  given  another  factor. 


♦►(composition) 


(decomposition)^ - 

-I  ■►©►© 
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its  covariance  as 


Zjk  ©Zjj  (B  zik 

Rj k  '^2© (R'ifc  Jl®RyJie)J2©  ) 


(3.10) 

(3.11) 


where  J2©  is  guaranteed  to  be  nonsingular  by  the  nature  of  the  compounding  operation — a  full-rank 
rigid-body  transformation.  We  call  this  operation,  ‘decomposition’ .  Composition  and  decomposi¬ 
tion  are  illustrated  in  Fig.  3.2. 

In  our  field  trials,  each  pose  is  parameterized  by  the  vehicle’s  XY  horizontal  position,  i.e.,  x,  e 
M2.  In  this  case,  J10  =  J2©  =  I,  and  composition  and  decomposition  reduce  to  simple  addition  and 
subtraction,  respectively.  Pauli  et  al.  (2014)  introduced  an  equivalent  composition/decomposition 
operation  applicable  in  only  M2. 

Any  link  in  the  local  chain  can  be  communicated  by  broadcasting  its  composed  odometry  to  an 
‘origin’  pose  (in  practice,  we  use  the  local  coordinate  frame  origin)  as  in  line  7  of  Algorithm  3.  The 
receiving  vehicle  simply  decomposes  the  odometry  factor  with  the  set  of  already  received  links  as 
in  line  5  of  Algorithm  4.  In  this  manner,  we  can  exactly  distribute  the  odometry  backbone  of  a 
local  graph.  The  subset  of  links  each  vehicle  has  received  is  unknown  to  the  broadcasting  vehicles, 
however,  composition/decomposition  still  allows  a  receiver  to  reconstruct  a  complete  chain  over 
received  links. 


Composed  Factor  Equivalence 

Here,  we  elaborate  on  the  claim  that  the  information  induced  by  the  composed  odometry  factor  is 
equivalent  to  the  marginal  information  induced  by  the  full  set  of  odometry  factors.  The  result  of 
this  claim  is  that  the  composed  odometry  factor  adds  exactly  the  same  information  as  the  original 
factors,  i.e.,  no  information  is  either  gained  or  lost.  Without  loss  of  generality,  we  consider  two 
odometry  factors:  zV]  between  poses  x,:  and  x? ,  and  z]k  between  poses  xy  and  x/,.  The  composed 
factor,  zik,  is  computed  through  (3.9). 

The  marginal  information  induced  by  ztJ  and  z]k  is  computed  by  constructing  the  full  potential 
and  then  marginalizing  over  the  intermediate  pose  node  x;,  i.e., 


Pip^i  !  Xfc  |  Z  y  , 


pip^-ij  X/ '  Yr  z }j ,  Zjk^)dx.j . 
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First,  the  information  induced  over  the  set  of  involved  pose  variable  nodes,  { x, ,  xy ,  x/, } ,  is  obtained 
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The  marginal  information,  A,/,,  is  then  computed  by  the  Schur  complement.  For  poses  in  Md, 
Jie  =  J2©  =  —  J©  =  I  and  the  marginal  information  induced  by  the  original  factors  is 


(Rjj  +  Rjfc)  1  —  (Ry  +  Rjfe)  1 

— (Rjj  +  R  jk)  1  (Rjj  +  R  jk)  1 


The  information  induced  by  the  composed  factor  z,;/.  is  easily  computed 


A ik  =  jTR7fclj 


JiffiJ© 


R7fe 


JiffiJ© 


J©Ji©Rifc  JieJe  J©Ji©Rifc  J2 


jJIffiRifc  Jl©J© 


J2ffiRifc 


(3.12) 


(3.13) 


where  R ik  is  determined  from  (3.9).  Again,  for  poses  in  M'7  we  can  compute  the  induced  informa¬ 
tion 


"  R^1 

~R7k 

(Rjj  +  R jk)  1 

—  (K,y  "  I!;/.-)  1 

rRik 

Rik  _ 

—(Rij  +  Kjk)  1 

(R  ij  +  R jk)  1 

(3.14) 


The  information  (3.14)  is  equivalent  to  the  marginal  information  induced  by  the  original  factors 
(3.13),  i.e.,  A ik  =  A'ik.  This  equivalence  can  also  be  verifed  for  poses  on  SE( 2)  and  SE( 3). 


3.3.2  Local  Chain  Approximation 

A  local  chain,  Clocal,  will  include  both  unary  prior  factors  and  pairwise  sequential  odometry  factors 
over  the  full  set  of  pose  nodes.  Ideally,  the  local  chain  would  only  include  pose  nodes  at  each 
TOL — the  only  poses  involved  in  relative  ranging  events  observed  by  other  vehicles — to  reduce 
the  size  of  the  chain  that  must  be  shared.  In  this  section,  we  detail  a  method  to  incrementally 
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Figure  3.3  At  the  new  TOL,  xk,  we  compute  a  set  of  factors  (orange)  that  best  represents  the  distribution  induced  by 
the  original  factors  (blue)  since  the  last  TOL,  x;/ ,  with  intermediate  nodes  (shaded)  marginalized  out. 

1 

( 

! 

( 

c|k!> 

a)  Local  chain,  C|oca],  over  all  poses. 

@ - . - @ - . - @ 

b)  Approximate  local  chain,  Capprox,  over  TOL  poses. 

1 

I 

approximate  the  full  local  chain  with  a  chain  only  over  TOL  pose  nodes,  Cappmx,  as  illustrated  in 
Fig.  3.3. 

We  refer  to  the  marginal  distribution  over  TOL  pose  nodes  computed  from  Clocal  as  the  target 
distribution.  The  marginal  cannot  be  exactly  represented  by  a  set  of  prior  and  odometry  factors  (a 
requirement  for  communication  by  odometry  composition,  see  Section  3.3.1).  However,  we  can 
compute  the  set  of  factors  that  most  closely  models  this  target  distribution.  The  general  framework 
for  representing  a  Gaussian  distribution  using  a  desired  nonlinear  factor  set  is  outlined  by  Mazuran 
et  al.  (2014). 

We  incrementally  construct  the  approximate  local  chain  by  ‘refactoring’  the  full  local  chain 
between  TOLs  as  in  Fig.  3.3.  The  links  contained  between  the  last  TOL  and  the  current  TOL 
induce  a  marginal  distribution  over  the  TOL  nodes  p(x.j,  xfc)  =  A f(/i,  E)  =  A/"-1  (ry,  A) ,  the  target 
distribution. 

We  compute  a  single  odometry  factor,  z]k  =  //odo  (x; .  xfc)  +  wjk,  and  a  prior  over  each  node, 
z j  =  /iprior (xd )  +  Wj  and  z/.  =  //.pno,- (x/,:)  +  Wfc,  that  most  closely  induce  the  target  distribution, 
p(x.j,  x*.).  Each  factor  is  perturbed  by  independent  zero-mean  Gaussian  noise  with  covariance 
given  by  Rjfc,  R;  and  Rfc.  Let  the  distribution  induced  by  the  approximate  factors  be 


g(Xj,xfc)  =Af  1  (V,  A') 

r\  =  JtR_1  (Z  —  /i(xj,Xj)) 
A'  =  JtR-1J 


Z  =  [zJfc>zJ>zfc]T 
R  =  diag(Rifc,  Rj,  Rk) 


J 


(//^  odo  /  //x.y  d  /i0do  /  dXj 

9  //prior  /  //x  j 


(3.15) 

(3.16) 

(3.17) 
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9h  prior/  //x/r 


where  Z  represents  the  stacked  observation  vector,  J  is  the  stacked  measurement  Jacobian,  and  R 
is  the  block-diagonal  stacked  measurement  covariance.  We  compute  Z  and  R  to  minimize  the 
Kullback-Leibler  divergence  (KLD)  between  p(x?,  x/j  and  q(x.j,  xfc) 

^kl  (p(xj,Xfe)||g(xj,Xfc))  =  ^  ^tr(A'E)  +  (n'  -  r)TA(//  -  p)  -  k  +  In  ,  (3-18) 

where  k  is  the  dimension  of  the  stacked  state,  i.e.,  dim(/tt).  The  KLD  is  minimized  when  Z  is 
the  expected  observation  evaluated  at  the  mean,  i.e.,  Z  =  /x  -)  such  that  \jt!  =  //.  R  is  then 
computed  from  (3.18) 


R*  =  argmintr  (jTR^JE)  -  lndet  (JTRJ)  ,  (3.19) 

ReS++ 

where  S++  is  the  cone  of  symmetric  positive  definite  matrices  (with  appropriate  sparsity  structure). 
As  shown  in  Mazuran  et  al.  (2014),  the  above  optimization  problem  is  convex  and  can  be  solved 
efficiently. 

We  have  not  enforced  a  consistency  constraint  on  R,  which  would  guarantee  that  the  approxi¬ 
mate  factors  are  not  overconfident.  The  consistency  constraint  can  be  expressed  as  a  linear  matrix 
inequality,  S'  —  E  A  0,  as  in  Carlevaris-Bianco  and  Eustice  (2014).  Although  an  explicit  consis¬ 
tency  constraint  was  not  employed  here,  the  approximation  produced  a  consistent  estimate  in  all 
field  trials. 

In  general,  A  will  not  be  full  rank  and  we  cannot  compute  approximate  factors  as  in  (3.19).  This 
will  only  occur  if  no  prior  is  available  over  the  time  window  i  — »  j  (as  is  often  true  for  AUVs). 
In  this  case,  however,  we  can  exactly  represent  the  target  information  with  a  composed  odometry 
factor  (see  Section  3.3.1)  and  no  prior  factors.  Similarly,  if  no  odometry  is  available  over  i  — >  j 
(e.g.,  for  a  topside  vehicle  with  only  GPS),  then  the  TOL  poses  are  independent  and  a  prior  may 
be  computed  to  exactly  represent  the  target  distribution  without  an  odometry  factor. 

At  the  TOL,  after  computing  new  approximate  link  factors  in  Capprox,  a  vehicle  broadcasts 
the  new  TOL  link  and  k  additional  links  to  rebroadcast,  L,  as  in  Algorithm  3.  The  size  of  k  is 
predetermined  depending  on  the  available  bandwidth. 

3.3.3  Rebroadcasting  Priors 

Capprox  is  reconstructed  from  the  set  of  received  links  and  therefore  may  not  contain  all  prior  factors. 
We  use  some  additional  bandwidth  to  rebroadcast  useful  or  informative  links  with  prior  factors. 
Below,  we  consider  finding  the  set  of  prior  factors  that  are  most  informative  about  the  current 
vehicle  pose  so  that  receiving  vehicles  are  able  to  best  use  their  observed  range  measurement. 
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A  simple  method  is  to  broadcast  the  k  most  recent  prior  factors  (A'- last  in  Section  3.4).  In¬ 
tuitively,  the  most  recent  prior  factors  may  be  most  informative  about  the  transmitting  vehicle’s 
current  pose.  However,  as  a  counterpoint,  consider  a  vehicle  with  perfect  (deterministic)  odome- 
try.  Then,  the  best  prior  factors  would  be  the  factors  with  lowest  uncertainty,  not  the  most  recent. 
For  this  reason,  we  develop  a  method  to  identify  a  more  informative  set  of  prior  factors  based  on 
the  expected  set  of  factors  already  received  by  the  network  (A;-best  in  Section  3.4). 

Since  each  prior  factor  is  broadcast  over  a  lossy  and  unacknowledged  channel,  we  assume  that 
the  zth  transmission  is  lost  with  probability  r%.  If  the  yth  link  has  been  broadcast  rn3  times,  then  the 
probability  that  it  has  been  received  is 


"l3 

Prtc:i  =  1  -  II  A-  (3.20) 

The  results  presented  in  Section  3.4  assume  that  r,  is  a  fixed  parameter;  however,  r,  could  in¬ 
corporate  additional  knowledge,  or  even  be  a  learned  parameter.  We  employ  a  similar  Bernoulli 
reception  model  in  Chapter  4. 

We  use  a  mutual  information  objective  for  determining  the  utility  of  a  prior  factor.  Mutual 
information  indicates  the  uncertainty  reduction  in  the  current  pose  of  the  reconstructed  chain,  x  Y, 
by  adding  knowledge  of  a  prior  factor.  Let  LK  =  {C^, ....  Cik}  C  (£i, ....  Cn}  be  the  set  of  k 
links  that  we  will  broadcast.  The  optimal  set  then  satisfies 


h*K  =  arg  max  E  [MI  [xjv  |  L k] } 

Lx 

=  arg  max  E  [log  det  AreCon(xzv)] 

L  K 


-A-recon 


all  odo  factors 


each  prior 


where  Arecon(xzv)  represents  the  marginal  information  ofxY  in  the  reconstructed  chain  computed 
from  Arecon  via  the  Schur  complement.  In  general,  this  is  a  difficult  combinatorial  optimization 
problem.  Instead,  we  follow  a  simpler  approach.  We  evaluate  the  objective  for  each  link  and  then 
greedily  select  the  single  link,  £,  ,  that  maximizes  the  objective,  increment  rnl,  and  remove  £,  from 
the  set  of  potential  links  to  broadcast.  We  repeat  the  process  of  evaluating  each  link  and  greedily 
selecting  the  best  k  times. 


3.3.4  Batch  Estimation 

As  mentioned  previously,  the  gold-standard  centralized  estimator  computes  the  MAP  configuration 
over  all  vehicle  poses  (3.3).  The  zth  vehicle,  however,  only  has  access  to  its  local  chain,  C|()cai(,  and 
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Algorithm  4  Cooperative  localization 

Require:  Ciocai  {Local  chain} 

1:  C  =  0  {Set  of  received  chains.} 

2:  Zr  =  0  {Set  of  received  range  observations.} 

3:  while  is_running()  do 

4:  if  Lrec,  zr,  i  =  received_broadcast()  then 

5:  add  links(C[i],  Lrec)  {Links  from  vehicle  i. } 

6.  f k  [•  LJ  zr 

7:  solve_batch(Ciocal5  C,  Zr) 

8:  end  if 

9:  end  while 


the  set  of  reconstructed  chains  from  broadcast  links.  Let  I  be  the  set  of  vehicle  indices  for  which 
ith  vehicle  has  reconstructed  a  chain.  The  full  posterior  (3.2)  is  approximated  by 

p(Xi,  Xiei|Z. j,  Zjei,  Zr)  (x  p(Xi  |Z.t)  ;  Z/)  JJp(zfc|x4,  XjJ.  (3.21) 

3TV  ^ jei v  fc 

L- local  ^  c^approx^- 

as  illustrated  in  Fig.  3.1b.  Note  that  the  posterior  is  only  computed  over  local  vehicle  poses  and 
the  set  of  received  broadcast  TOL  poses  and  that  only  the  set  of  locally  obtained  relative  pose 
constraints  is  used.  The  vehicle  can  then  employ  any  graph-based  solver  (e.g.,  square-root  SAM) 
to  compute  a  solution  (e.g.,  see  Appendix  A).  Algorithm  4  reviews  the  estimation  procedure  per¬ 
formed  by  each  vehicle. 


3.4  Field  Trials 

For  validation,  we  fielded  two  Ocean-Server  Inc.  Iver2  AUVs,  termed  AUV-A  and  AUV-B  (see 
Fig.  3.4),  and  a  topside  support  ship.  The  AUVs  are  each  equipped  with  an  advanced  dead- 
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reckoning  sensor  suite.  Each  AUV  observed  GPS  during  intermittent  surface  intervals.  The  topside 
support  ship  had  access  only  to  GPS  (no  measured  odometry).  All  vehicles  used  the  WHOI  Micro¬ 
modem  and  co-processor  board  with  a  synchronous-clock  reference  for  inter  vehicle  communica¬ 
tion  and  ranging.  The  AUV  hardware  is  detailed  in  Appendix  C.  Experiments  were  conducted  July 
25-27,  2014,  at  the  UMBS. 

Each  vehicle  pose  was  parameterized  by  its  XY  horizontal  position.  GPS  observations  were 
transformed  to  a  local  coordinate  frame  and  constitute  a  full-rank  linear  observation.  We  assumed 
that  the  GPS  accuracy  was  constant  and  therefore  we  assumed  noise  with  a  fixed  standard  de¬ 
viation  of  5  m.  The  AUV  odometry,  z(J,  and  corresponding  covariance,  Ri?,  were  computed  by 
Euler  integrating  DVL  and  AHRS  and  performing  a  first-order  covariance  approximation  (see  Ap¬ 
pendix  B.l). 

Observed  OWTT  ranges  represent  a  3D  slant  range.  Since  depth  is  measured  with  bounded  er¬ 
ror,  we  are  able  to  project  ranges  into  the  2D  horizontal  plane  and  use  the  resulting  pseudo  ranges 
within  our  estimation  framework  (Appendix  B.2).  We  used  a  fixed  OWTT  measurement  uncer¬ 
tainty  since  the  relative  depth  between  each  vehicle’s  acoustic  transducer  was  small  and  relatively 
constant. 

Vehicles  broadcast  navigation  messages  roughly  once  per  minute  according  to  a  fixed  TDMA 
schedule.  The  proposed  composition/decomposition  allowed  a  coarse  quantization  (factors  were 
rounded  to  1  cm).  Each  navigation  packet  (i.e.,  set  of  links  L)  required  a  single  64  B  frame  of 
either  a  Micro-modem  Rate  1  or  Rate  2  data  packet.  The  previously  detailed  OSM  (Chapter  2) 
minimally  requires  two  64  B  frames,  leading  to  a  50%  reduction.  The  full  message  specification  is 
detailed  in  Appendix  C. 

We  employed  our  /c-last  prior  factor  selection  strategy  during  real-time  experiments  (for  k  —  2). 
In  post-process,  we  computed  the  prior  factors  that  would  have  been  broadcast  with  our  k- best 
strategy. 

We  also  implemented  the  algorithm  proposed  by  Pauli  et  al.  (2014)  for  comparison.  Their 
algorithm  similarly  broadcasts  each  new  TOL  pose  as  a  composed  factor.  There  are  two  large  dif¬ 
ferences  in  our  approach:  (i)  they  transmit  prior  factors  directly  from  the  full  local  chain  separately, 
i.e.,  they  must  broadcast  each  prior  link  in  addition  to  TOL  links,  and  (ii),  they  also  broadcast  re¬ 
ceived  range  factors  and  their  corresponding  TOA  links.  The  second  difference  effectively  allows 
for  bidirectional  ranging  at  the  cost  of  packets  that  grow  linearly  in  the  size  of  the  network  (a  TOA 
pose  is  broadcast  for  each  vehicle  in  the  network).  During  the  three  vehicle  post-process  eval¬ 
uation,  their  algorithm  broadcast  more  links  requiring  greater  bandwidth  (4  composed  odometry 
factors,  2  range  factors,  and  a  prior  factor  compared  to  3  composed  odometry  factors  and  2  prior 
factors  for  ours). 
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Figure  3.5  Local  graph  reconstruction  quality  for  A:- last,  fc-best,  and  Pauli  et  al.  (2014)  for  various  reception  rates 
averaged  over  100  trials  for  six  local  chains.  Note  log  scale  on  ordinate  axis. 


Reception  rate 

(a)  Mean  difference  in  pose  between  target  and  reconstructed. 


Reception  rate 

(b)  KLD  between  target  distribution  and  reconstructed. 


3.4.1  Local  graph  reconstruction 

Here  we  demonstrate  the  quality  of  the  local  chain  reconstruction  under  varying  channel  condi¬ 
tions.  We  ran  three  trials  ranging  from  60  —  90  min  to  collect  navigation  observations  and  broad¬ 
cast  event  times  for  each  of  our  two  AUVs  resulting  in  six  local  chains.  During  the  trials,  each 
AUV  had  intermittent  access  to  GPS  during  brief  surface  intervals.  In  post-process,  we  computed 
and  sampled  the  set  of  navigation  packets  at  different  reception  rates  and  reconstructed  the  local 
chain.  For  each  reception  rate,  we  artificially  sampled  different  sets  of  received  messages  100 
times.  We  then  compared  the  reconstructed  local  chain  to  the  target  chain  (the  marginal  full  chain 
over  received  TOL  pose  nodes).  Fig.  3.5  compares  the  local  chain  reconstruction  for  prior  factor 
selection  methods,  fc- last  and  k- best,  as  well  as  the  method  proposed  by  Pauli  et  al.  (2014). 

The  fc-best  selection  strategy  outperforms  fc-last  for  both  mean  reconstruction  error  and  KLD 
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Table  3.1  Acoustic  communication  reception  rates  between  pairs  of  vehicles  across  all  trials. 


Trial  1 

Trial  2 

Trial  3 

AUV-A  fv  AUV-B 

57.0% 

62.9% 

37.6% 

AUV-A  fv  Topside 

82.8% 

74.9% 

52.9% 

AUV-B  fv  Topside 

84.8% 

78.3% 

86.6% 

by  a  factor  of  two  or  three  in  most  trials.  At  100%  reception  rate  (no  dropped  packets)  the  full 
approximate  chain  was  reconstructed  and  is  therefore  equivalent  for  k- best  and  A  - last.  Moreover, 
the  full  approximate  chain  closely  represents  the  full  local  chain.  While  k- best  outperforms  Ac-last, 
k- last  has  a  computational  advantage  incurring  essentially  no  cost  to  choose  the  last  several  priors. 

Our  proposed  algorithm  improves  upon  Pauli  et  al.’s  method  by  several  orders  of  magnitude 
in  most  trials.  Our  method  is  able  to  include  more  prior  information  into  the  broadcast  links,  and 
therefore  leads  to  a  better  reconstruction. 

As  mentioned  in  Section  3.3.1,  composition  is  equivalent  to  marginalization.  Therefore,  when 
no  priors  are  present  in  the  local  chain,  all  compared  algorithms  are  exactly  equal  to  the  target 
distribution  (up  to  quantization  errors). 

3.4.2  Cooperative  localization 

We  executed  three  multiple  vehicle  trials  to  demonstrate  the  ability  of  our  algorithm  to  provide 
useful  navigation  information  to  an  AUV.  We  varied  the  number  of  vehicles  and  AUV  access  to 
GPS  in  each  trial  to  demonstrate  a  variety  of  practical  applications.  Acoustic  reception  rates  varied 
between  37-86%  up  to  500  m  relative  range  as  summarized  in  Table  3.1.  Results  are  summarized 
in  Fig.  3.6.  Fig.  3.2  reports  each  distributed  algorithm’s  ability  to  produce  the  centralized  uncer¬ 
tainty  estimate  onboard  AUV-B.  Although  we  only  show  results  for  AUV-B,  note  that  all  vehicles 
compute  a  local  reconstruction  of  the  centralized  estimator. 

Trial  1 

AUV-A  and  AUV-B  performed  overlapping  orthogonal  lawnmower  surveys  (see  Fig.  3.6a).  Both 
vehicles  had  only  dead-reckoned  navigation  available.  Since  there  are  no  GPS  priors,  our  local 
graph  reconstruction  is  equivalent  to  Pauli  et  al.  (2014).  Moreover,  the  local  chain  reconstruction 


Table  3.2  Average  1-cr  uncertainty  difference  compared  to  the  centralized  estimator  across  all  trials. 


Trial  1  [  m] 

Trial  2  [  m] 

Trial  3  [  m] 

Pauli  et  al.  (2014) 

0.005 

0.981 

0.463 

Proposed  (Ac-last) 

0.039 

0.086 

0.212 

Proposed  (k- best) 

0.039 

0.074 

0.192 

69 


Figure  3.6  Summary  of  field  trials  and  performance  comparison,  (a-c)  shows  each  vehicle  trajectory,  (d-f)  plots 
the  smoothed  uncertainty  in  each  AUV-B  pose  computed  as  the  fourth  root  of  the  determinant  of  the  pose  marginal 
covariance.  AUV-A  access  to  GPS  is  indicated  by  green  boxes. 


“200  -100  0  100 
y  [in]  y  [m] 

(a)  Trial  1:  relative  paths  (1.17  h),  (b)  Trial  2:  relative  paths  (1.50  h), 
no  access  to  GPS.  AUV-A  has  access  to  GPS. 


AUV-A,  topside  (not  shown) 
have  GPS. 


Dead-reckoned  ' 
Centralized 
A: -best 
A; -last 
Pauli  et  al. 

4000 


(d)  Trial  1:  AUV-B’s  uncertainty,  (e)  Trial  2:  AUV-B’s  uncertainty,  (f)  Trial  3:  AUV-B’s  uncertainty. 


0  1000  2000  3000  4000 


mission  time  [s] 

(g)  Trial  1 :  Difference  in  uncer¬ 
tainty  compared  to  centralized 
estimator. 


mission  time  [s] 

(h)  Trial  2:  Difference  in  uncer¬ 
tainty  compared  to  centralized 
estimator. 


mission  time  [s] 

(i)  Trial  3:  Difference  in  uncer¬ 
tainty  compared  to  centralized 
estimator. 


for  all  algorithms  was  exact,  /e-last  and  A; -best  produced  the  same  solution  since  there  are  no  priors 
to  rebroadcast.  Since  Pauli  et  al.  (2014)  incorporated  range  constraints  between  both  vehicles, 
their  algorithm  computed  a  solution  that  more  closely  matches  the  centralized  result  (see  Fig.  3.2). 
Their  algorithm  did  not  exactly  reproduce  the  centralized  result  because  AUV-B  did  not  receive 
all  range  factors  observed  by  AUV-A.  Although  we  only  used  the  local  subset  of  range  factors, 
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we  were  still  able  to  benefit  from  relative  range  observations  and  the  difference  compared  to  the 
centralized  estimator  is  small. 

Trial  2 

AUV-A  executed  a  narrow  diamond  trajectory  over  AUV-B’s  lawnmower  survey.  Both  vehicles 
had  dead-reckoned  navigation  available,  but  AUV-A  also  intermittently  received  GPS  during  short 
surface  intervals  (see  Fig.  3.6b).  Fig.  3.6e  plots  the  pose  uncertainty  onboard  AUV-B.  Using  our 
proposed  algorithm,  AUV-B  was  able  to  accurately  approximate  AUV-A’s  pose-graph,  including 
the  many  prior  factors,  k- best  here  performed  slightly  better  than  A  - last  because  of  its  ability 
to  more  accurately  reproduce  AUV-A’s  local  chain.  Although  Pauli  et  al.  (2014)  were  able  to 
incorporate  range  information  in  both  directions,  our  reconstruction  of  AUV-A  was  more  accurate, 
leading  to  a  more  confident  estimate.  Due  to  the  diminishing  returns  of  information  provided  by 
the  range  observations,  our  algorithm  was  able  to  closely  reproduce  the  centralized  solution  despite 
only  using  a  subset  of  the  range  observations. 

Trial  3 

A  topside  vehicle  with  constant  GPS  access  supported  AUV-A  (with  intermittent  GPS)  and  AUV- 
B  (see  Fig.  3.6c).  The  factor  graph  produced  here  is  illustrated  in  Fig.  3.1.  AUV-A  followed  a  large 
diamond  over  AUV-B’s  lawnmower  survey  while  the  topside  vehicle  drifted  above  the  survey  area. 
In  this  case,  the  centralized  estimator  used  ranges  between  all  three  vehicles.  Our  method  only  used 
ranges  between  the  local  platform  and  the  other  vehicles.  Pauli  et  al.  (2014),  however,  included 
ranges  in  both  directions  and  could  also  use  ranges  between  other  vehicles,  but  only  if  the  TOL 
and  TOA  poses  in  each  local  chain  had  been  received.  Once  again,  our  reconstruction  of  AUV-A’s 
chain  is  more  informative.  As  shown  in  Fig.  3.2,  we  have  received  the  bulk  of  the  benefit  in  terms 
of  uncertainty  reduction  from  the  local  set  of  range  observations  and  achieved  a  smaller  estimate 
uncertainty  lower  than  Pauli  et  al.  (2014). 

Summary 

In  all  field  trials,  the  presented  factor  composition  method  produces  an  accurate  representation 
of  the  centralized  estimator.  Moreover,  both  k- best  and  A'- last  methods  produced  a  consistent 
estimate — they  were  never  more  confident  than  the  centralized  estimator.  By  applying  an  approx¬ 
imate  marginalization  procedure,  we  can  broadcast  a  small  yet  informative  set  of  factors.  The 
results  also  demonstrate  that  the  local  set  of  range  observations  is  capable  of  bounding  uncertainty 
growth. 
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Figure  3.7  Comparison  of  reconstructed  local  chains  by  Pauli  et  al.  (b)  and  our  method  (c)  as  if  the  data  transmitted  at 
the  second  TOL,  Xj,  was  not  received.  Arrows  away  from  the  chain  indicate  TOLs,  arrows  toward  the  chain  indicate 
TOAs. 


(a)  True  local  chain,  Ciocai,  over  all  poses. 


(b)  Local  chain  reconstructed  by  Pauli  et  al.  including  a 
range  observed  by  the  other  vehicle  at  a  TOA  pose. 

t  ?  t 


(c)  Approximate  local  chain,  Capprox>  reconstructed  by  our 


method,  /c-last,  with  k  >  1. 


3.5  Discussion 

Our  proposed  algorithm  is  similar  in  some  respects  to  that  independently  introduced  by  Pauli  et  al. 
(2014),  however,  we  believe  there  are  many  beneficial  differences  illustrated  in  Fig.  3.7.  The  most 
marked  improvement  in  our  algorithm  is  its  ability  to  broadcast  a  local  chain  by  using  an  accurate 
approximation  that  includes  a  more  informative  set  of  prior  factors.  The  compact  approximate 
chain  contains  fewer  links  to  broadcast.  While  we  currently  only  incorporate  ranges  measured 
locally,  this  allows  for  a  fixed  bandwidth  data  packet,  regardless  of  the  size  of  the  vehicle  network. 
We  can  use  the  additional  bandwidth  to  rebroadcast  previous  informative  links  and  for  other  prac¬ 
tical  purposes  (e.g.,  command,  control,  vehicle  health).  Pauli  et  al.  (2014)  are  able  to  incorporate 
inter-vehicle  ranges,  but  only  when  the  receiving  vehicle  has  received  all  involved  poses.  Both 
methods  do  not  support  cascaded  communication  topologies  (one  vehicle  supports  another,  which 
in  turn  supports  another).  Finally,  the  local  subset  of  range  observations  is  sufficient  for  accurate 
navigation  in  many  practical  situations  (as  evidenced  in  Section  3.4). 

3.6  Chapter  Summary 

Accurate  localization  extends  the  capacity  of  AUVs  to  perform  ocean  science.  OWTT  underwa¬ 
ter  cooperative  localization  promises  improved  navigation  for  AUVs  over  larger  area  and  time 
scales  without  additional  infrastructure.  We  exploit  the  structure  of  the  composition  operation  over 
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odometry  factors  and  an  accurate  approximation  of  the  local  chain  (factor  graph)  to  share  locally 
observed  sensor  data  across  a  fragile  communication  channel.  That  is,  any  pose  node  in  the  chain 
can  be  represented  with  its  composed  odometry  link.  We  then  use  the  collection  of  received  chains 
and  observed  relative  pose  measurements  to  compute  an  improved  navigation  solution.  The  pro¬ 
posed  method  scales  to  many  vehicles  in  that  each  vehicle  in  the  network  can  simultaneously  act 
as  server  and  client.  Compared  to  the  previous  server-client  architecture  introduced  in  Chapter  2, 
the  proposed  method  is  more  tolerant  to  communication  failure  as  a  recovery  mechanism  is  not 
required. 

Our  proposed  method  can  also  find  application  in  the  broader  robotics  community  where  lim¬ 
ited  communication  is  an  operational  factor,  e.g.,  search  and  rescue.  Avenues  for  future  work 
include  extending  our  chain  approximation  method  to  include  information  received  from  other  ve¬ 
hicles.  This  would  allow  better  localization  performance  in  certain  communication  topologies,  for 
example,  cascaded  networks. 
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Chapter  4 

Planning  for  Cooperative  Localization 


In  this  chapter,  we  assume  that  a  distributed  estimation  framework  for  server-client  localization 
(such  as  those  developed  in  Chapter  2  and  Chapter  3)  is  available  and  examine  how  the  quality  of 
the  client  estimate  varies  with  the  vehicle  network  geometry1.  We  then  use  this  to  plan  a  server 
vehicle  path  for  cooperative  localization.  We  assume  that  the  client  mission  plan,  i.e.,  desired 
trajectory,  is  available  at  planning  time.  Our  goal  is  to  compute  a  practical  server  trajectory  that 
minimizes  the  predicted  client  uncertainty  through  OWTT  support. 

Within  OWTT  cooperative  localization  there  is  an  inherent  trade-off  between  communication 
fidelity  and  localization  accuracy — increasing  horizontal  relative  range  diminishes  the  probabil¬ 
ity  of  successful  communication,  but  improves  horizontal  position  estimation,  as  illustrated  in 
Fig.  4.1.  Therefore,  planning  serves  to  benefit  from  modeling  the  communication  channel.  Belief 
space  planning  represents  a  principled  approach  to  balancing  standard  path-planning  objectives 
(e.g.,  minimum  distance)  with  motion,  sensing,  and,  in  this  chapter,  communication  uncertainty. 
Resulting  paths  seek  information  in  order  to  reduce  state  uncertainty  and  successfully  meet  a  task- 
specific  objective. 

Belief  space  planning  solves  a  more  general  POMDP  problem  by  making  a  linear  Gaussian  as¬ 
sumption.  Most  belief  space  planning  approaches,  however,  ignore  that  sensor  measurements  may 
not  be  obtained,  for  example,  because  of  a  lossy  communication  channel.  We  introduce  a  model 
of  the  underwater  acoustic  communication  channel  into  belief  space  planning  that  allows  a  trajec¬ 
tory  optimization  algorithm  to  find  server  paths  that  provide  useful  navigation  information  and  are 
also  likely  to  demonstrate  successful  communication  (and  hence  provide  informative  relative  range 
observations). 

This  chapter  provides  the  following  contributions: 

•  We  propose  a  belief  space  planning  framework  for  server-client  cooperative  localization  with 
'Portions  of  this  work  have  previously  appeared  in  (Walls  and  Eustice,  2014b;  Walls  et  al.,  2015a). 
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Figure  4.1  Cooperative  localization  example:  a  server  vehicle  ‘A’  or  ‘B’  can  provide  a  relative  range  constraint  (white 
arrows)  to  the  client  vehicle  (yellow).  ‘A’  is  more  likely  to  make  the  range  observation  as  evidenced  by  the  channel 
gradient  (darker  means  higher  probability  of  making  a  measurement).  ‘B’,  however,  provides  more  information  in  the 
horizontal  plane  (as  indicated  by  the  thicker  arrow).  Our  proposed  planning  framework  balances  these  objectives. 


a  known  client  mission  plan. 

•  We  exploit  a  modified  observation  likelihood  function  that  allows  us  to  model  randomness 
in  obtaining  measurements.  We  then  employ  this  modified  observation  function  to  model 
the  underwater  acoustic  channel. 

•  We  propose  an  optimization  algorithm  for  computing  locally  optimal  receding-horizon  con¬ 
trol  actions  or  path  parameters  that  represent  practical  AUV  trajectories. 


Additionally,  we  provide  a  channel  model  that  is  representative  of  data  collected  during  AUV  field 
trials.  We  then  use  the  channel  model  to  demonstrate  our  planning  framework  in  several  simulated 
scenarios. 


4.1  Related  Work 

Path  planning  problems  typically  seek  a  feasible  path  from  start  to  goal  positions  within  a  con¬ 
figuration  space.  When  state  is  not  known  deterministically,  planning  can  be  formalized  within  a 
POMDP  framework  (Kaelbling  et  al.,  1998).  In  this  case,  objective  functions  are  defined  over  the 
space  of  potential  distributions  over  state,  called  the  belief  space.  Planning  methods  must  cope 
with  uncertainty  in  motion,  sensing,  and  the  surrounding  environment.  Due  to  the  high  dimension¬ 
ality  of  the  belief  spaces  of  real-world  problems,  solutions  to  POMDP  instances  must  typically  be 
approximated. 

Several  prior  approaches  to  belief  space  planning  (Prentice  and  Roy,  2009;  Agha-mohammadi 
et  al.,  2014;  Bry  and  Roy,  2011)  attempted  to  solve  belief  space  planning  problems  with  sampling- 
based  planners.  They  addressed  uncertainty  in  sensing,  motion,  or  both.  These  methods  involve 
constructing  candidate  trajectories  through  the  underlying  state  space  by  random  sampling,  then 
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searching  over  candidate  solutions.  These  algorithms  find  the  best  solution  within  the  space  of 
candidate  trajectories  but  do  not  explicitly  optimize  the  trajectory. 

Recently,  optimization  motion  planning  has  been  applied  to  belief  space  planning  (Platt  et  al., 
2010;  van  den  Berg  et  al.,  2012;  Patil  et  al.,  2014).  This  chapters  builds  off  of  these  methods 
in  order  to  optimize  over  both  open-loop  control  actions  and  parameterized  trajectories.  Several 
methods  (Platt  et  al.,  2010;  Patil  et  al.,  2014)  assume  maximum-likelihood  observations  in  order  to 
achieve  rapid  replanning.  Van  den  Berg  et  al.  (2012)  drop  the  maximum-likelihood  assumption  at 
the  cost  of  greater  computational  expense,  but  planning  is  performed  off-line  and  a  control  policy 
is  executed  at  run  time. 

The  related  area  of  active  SLAM  research  has  focused  on  computing  paths  that  lead  to  lower 
uncertainty  maps.  Stachniss  et  al.  (2005)  presented  an  early  particle-based  approach  to  make 
informative  decisions  under  uncertainty,  integrating  localization,  mapping,  and  exploration.  Sim 
and  Roy  (2005)  proposed  a  breadth-first  search  procedure  for  minimizing  an  A-optimal  objective 
on  vehicle  uncertainty.  Recently,  Indelman  et  al.  (2015)  merged  optimization-based  belief  space 
planning  within  active  SLAM.  These  authors  additionally  model  unknown  observation  events 
(similar  in  spirit  to  the  channel  model  presented  here),  but  employ  a  simplification  that  results  in  a 
deterministic  state  covariance. 

The  multiple  vehicle  coordination  problem  is  well  studied  (Kumar  et  al.,  2004),  however,  navi¬ 
gation  for  localization  remains  an  open  challenge.  Martinez  and  Bullo  (2006)  proposed  a  heuristic 
coordination  algorithm  for  target-tracking  with  a  range-only  sensor.  Charrow  et  al.  (2014)  sim¬ 
ilarly  presented  a  target-localization  control  policy  using  range-only  sensors.  The  most  relevant 
approaches  to  the  underwater  range-only  localization  were  presented  by  German  et  al.  (2012),  Tan 
et  al.  (2014),  and  Bahr  et  al.  (2012).  These  prior  works,  however,  do  not  include  motion  and  sens¬ 
ing  uncertainty  and,  in  some  cases,  consider  only  discrete  action  spaces.  Moreover,  no  prior  work 
has  modeled  the  communication  channel,  although  several  authors  have  imposed  a  heuristic  maxi¬ 
mum  communication  range,  e.g.,  Tan  et  al.  (2014).  We  propose  a  principled  method  to  incorporate 
channel  behavior  during  planning. 

4.2  Belief  Space  Planning 

The  general  planning  problem  we  address  in  this  chapter  is  an  instance  of  a  POMDP,  since  both 
sensing  and  motion  are  stochastic.  We  first  state  our  problem  instance  within  a  Gaussian  be¬ 
lief  space  (Section  4.2.1)  and  derive  the  belief  evolution  (Section  4.2.2).  Then,  we  modify  the 
belief  representation  to  handle  nondeterministic  observation  events  (Section  4.2.3).  Finally,  we 
develop  a  planning  framework  that  leverages  this  modified  belief  representation  (Section  4.2.4  and 
Section  4.2.5). 
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4.2.1  Gaussian  Belief  Space  Planning 


Consider  a  system  with  state  x^,  at  time  step  k.  The  state  transition  model  captures  uncertainty  in 
executing  an  action, 


xfe+i  =  /(xfc,Ufc,wfc),  (4.1) 

where  uk  is  the  control  input,  and  wk  ~  p( wfc|xfc,  uk)  is  an  independent  noise  disturbance  with 
known  density,  assumed  here  to  be  zero-mean  Gaussian  with  covariance  matrix  Mk.  This  transition 
model  equivalently  encodes  the  transition  distribution  p(xfc+i|xfe,  uk)  =  A /”(/(xfc,  uk,  0),  Mfc). 
Observations  are  also  random  variables  and  modeled  as 


zfc  =  /r(xfc,  vfc),  (4.2) 

where  v/,.  ~  p(vfc  x/,)  is  an  independent  noise  perturbation  with  known  density.  We  assume  that  the 
observation  noise  is  additive  and  drawn  from  a  zero-mean  Gaussian  distribution  with  covariance 
matrix  N^.  The  observation  model  is  then  represented  by  the  likelihood  function  p(z/,.|x/.)  = 

jV(/i(xfc,0),Njfc). 

The  belief  b/,  is  the  posterior  distribution  over  state  given  all  observations  and  control  inputs 
up  to  time  k,  i.e., 


b k  —  P(xfc|Zl:fc,  Ui:fc_i), 


(4.3) 


where  Z1:k  =  { z?;  }  J'= ,  is  the  set  of  observations  and  U  |:/,_]  =  {ii,} -'T,1  is  the  set  of  all  control 
inputs.  In  Section  4.2.3,  we  modify  the  observation  and  belief  definitions  to  include  unknown 
observation  events. 

In  this  work,  we  consider  a  general  quadratic  cost  function  comprised  of  the  sum  of  a  terminal 
cost  and  stage  costs  over  the  beliefs  and  controls  through  time-horizon  L 


min  E 


L—l 

+  y:  bjQfcbfc  +  ujRfcUfc  , 

k= 1 


(4.4) 


where  Bi :L  =  {bfc}|'=1  is  the  set  of  belief  states  that  satisfy  the  relationship  in  (4.3),  and  the 
expectation  is  taken  with  respect  to  the  random  observations,  Zi  ^.  The  cost  weight  matrices  Qa,- 
and  Rfc  are  positive  semi-definite  and  positive  definite,  respectively.  Quadratic  (or  approximately 
quadratic)  cost  functions  are  commonly  employed  in  belief  space  planning  problems  for  the  relative 
ease  with  which  they  can  be  minimized. 

Computing  the  optimal  solution  to  a  cost  function  over  belief  states  is  generally  intractable 
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because  of  the  high  dimensionality  of  the  space  of  all  beliefs.  Under  the  assumption  of  additive 
Gaussian  noise  and  locally  linear  models,  however,  a  belief  is  then  approximately  Gaussian  and 
can  be  parameterized  by  the  state  mean  x*.  and  vectorized  covariance  matrix  £&, 


bfc 


vec(£fc)T 


(4.5) 


Note  that  the  vec(  • )  operation  exploits  the  symmetry  in  £fc.  This  Gaussian  parameterization  en¬ 
ables  tractable  optimization. 

Our  strategy  closely  follows  previous  methods  for  deriving  an  analytical  belief  evolution  and 
optimizing  within  that  framework  (He  et  al.,  2011;  van  den  Berg  et  al.,  2012).  We  proceed  in 
two  steps.  First,  we  derive  the  belief  dynamics,  which  define  the  belief  transition  distribution 
p(bfc+i|bfc,  Ufc).  Second,  we  use  the  predictive  belief  dynamics  to  minimize  a  cost  function  over 
beliefs  and  controls  (4.4).  Our  approach  differs  from  the  aforementioned  works  with  the  introduc¬ 
tion  of  a  channel  model  and  uses  an  alternative  optimization  framework  that  allows  simple  and 
practical  parametric  trajectories  to  be  considered. 


4.2.2  Belief  Dynamics 

The  belief  dynamics  detail  the  evolution  of  beliefs  given  a  control  sequence.  For  discrete  control 
spaces,  we  could  enumerate  all  possible  sequences.  However,  brute  force  enumeration  may  be 
intractable  for  even  moderate  horizon  lengths.  For  continuous  control  spaces,  enumeration  is  not 
possible.  Below  we  show  that  we  can  analytically  compute  the  distribution  over  beliefs. 

A  belief  b/.  represents  the  posterior  distribution  over  state.  The  belief  dynamics  are  computed 
via  a  recursive  Bayes  filter 


bfc+i  =  p(xfc+i|Zi:fc+ij-Ui:fc)  (4.6) 

=  rjp(zk+i\xk+i)p{xk+1\xk,uk,Z1:k), 

V - V - ' 

bfc+i 

where  b/,+i  is  the  predicted  belief  given  b /,.  and  u/,.  and  r/  is  a  normalization  constant. 

The  belief  dynamics  can  be  computed  in  closed  form  within  an  EKF  under  the  assumption  of 
Gaussian  noise  models  and  approximately  locally  linear  transition  and  observation  functions.  The 
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EKF  update  given  the  current  belief  follows 


(4.7) 

(4.8) 


Xfc+i  =  /(xfc,  ufc,0)  +  K^zk+1  -  /r(/(xfc,ufc,0),0)  j 
Sfc+1  =  (I  -  KH)S 
F  =  ^/(xfc,ufc,0) 

H  =  ^/i(/(xfc,  ufc,0),0)) 

S  =  FSfcFT  +  Mfc 
K  =  SHt(H£Ht  +  Nfc)_1. 


The  mean  update  (4.7)  is  random  in  z,  while  the  covariance  update  (4.8)  is  deterministic.  More¬ 
over,  the  distribution  over  the  belief  mean  is  Gaussian  since  z  is  Gaussian.  The  distribution  over 
beliefs — the  belief  dynamics — can  therefore  be  represented  by  a  Gaussian  process 


bfc+i  =  g(bk,uk)  +  W(bk,uk)wk 


g{ bfc,  ufe) 


W(b*;,  u*;) 


/(xfc,ufc,0) 
vec(S  -  KHS) 

Vkhe’ 


(4.9) 


where  ~  Af( 0, 1)  and  yf  ~  is  a  suitable  matrix  square-root  factor.  The  sequence  of  distributions 

over  beliefs  computed  through  the  belief  dynamics  is  illustrated  in  Fig.  4.2a. 


4.2.3  Belief  Dynamics  with  Uncertain  Channel  State 

Prior  belief  space  planning  methods  (as  above)  assume  that  the  set  of  future  observation  events  is 
known.  Whether  an  observation  is  made  or  not,  however,  is  often  not  known  before  execution  time. 
For  example,  the  set  of  range  observations  that  occur  in  an  underwater  acoustic  network  depends 
on  the  (unknown)  set  of  successful  acoustic  transmissions. 

To  address  this,  we  introduce  a  binary  channel  state  variable,  7^.  ~  Bemoulli(Afc),  which  mod¬ 
els  the  event  that  an  observation  is  received  at  the  /cth  time  index.  Sinopoli  et  al.  (2004)  introduced 
an  equivalent  model  for  studying  convergence  properties  of  the  EKF  error  covariance  in  sensor 
networks  with  fading  channels.  We  modify  the  observation  model  (4.2)  by  conditioning  on  the 
channel  state  variable  ‘switch’ 

p(zfc|xfc,  7*;)  =  A/”(/r(xfc,0),7fcNfc  +  (1  -  7fc)cr2l),  (4.10) 
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Figure  4.2  Illustration  of  belief  dynamics,  (a)  represents  standard  Gaussian  belief  dynamics,  resulting  in  a  distribution 
over  possible  mean  states.  Covariance  in  this  case  is  deterministic,  (b)  depicts  the  belief  dynamics  evolution  including 
an  unknown  channel  state — the  belief  state  branches  for  every  possible  channel  state.  Given  the  sequence  of  channel 
states,  the  belief  dynamics  are  equivalent  to  (a),  (c)  shows  our  approximation  of  (b),  which  can  be  tractably  computed. 


(a)  Linear  Gaussian  belief  dynamics.  Red  illustrates  distribution  over 


belief  mean  states. 


— 0^ 


_  L  1  A  _i  _  L  L 


u2r  72 


u2r  72 


.LIJJ. 


(b)  Linear  Gaussian  belief  dynamics  with  a  channel  state.  The  belief 
branches  based  on  channel  state. 


(c)  Approximate  linear  Gaussian  belief  dynamics  with  a  channel 
state.  Blue  represents  a  distribution  over  covariance. 


where  we  take  the  limit  as  a2  — >  oo.  If  the  observation  is  received  (7*.  =  1),  the  revised  model  is 
exactly  (4.2).  When  the  observation  is  not  received  (7*.  =  0),  the  observation  has  infinite  uncer¬ 
tainty  or,  equivalently,  zero  information.  Note  that  the  observation  model  is  again  represented  by 
a  Gaussian  distribution. 

The  belief  now  represents  the  posterior  distribution  over  state  given  all  channel  states  in  addi¬ 
tion  to  observations  and  control  inputs 

b/t  =  p(x*|r1:fc,  Z1:fc,Ui:fc_i),  (4.11) 

where  17^  =  { 7i } ?=1  -  By  leveraging  the  Markov  property  of  the  state  transition  model,  the  belief 
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dynamics  can  be  computed  with  a  recursive  Bayes  filter 


bfc+l  —  p(xfc+l|ri;fc+l,  Zi:fc+1,  u1:fc)  (4.12) 

VP{Zk+l  l^fe+l i  T/fc+ 1  )p(x/j_|-i  |Xfc ,  Ufc,  ri:fc, 

where  q  is  a  normalization  constant  and  we  assume  that  the  set  of  channel  states  is  independent  of 
the  state,  i.e.,  p(7i|x;)  =  p(7i).  While  the  channel  state  may  certainly  depend  on  the  underlying 
state,  we  have  assumed  independence  to  simplify  the  belief  dynamics  and  so  that  the  belief  is  not 
informed  by  whether  an  observation  is  received  or  not — the  channel  model  is  only  used  at  planning 
time  and  is  uninformative  during  execution. 

We  track  the  evolution  of  belief  states  (4.12)  with  an  EKF.  We  evaluate  the  state  prediction  and 
measurement  update  in  the  limit  by  substituting  the  observation  model  (4.10)  into  the  Bayes  filter 
(4.12)  and  taking  the  limit  as  a2  — >  oo.  The  EKF  update  given  the  current  belief  closely  follows 
the  standard  EKF  update 

xfc+i  =  /(xfc,  Ufc,  0)  +  7k(z  -  h(f(kk,  Ufc,  0),  0) ) 

Efc+1  =  (I-7KH)E 
F  =  ^/(xfc,  ufc,0) 

H  =  7^/i(/(xfc,  ufc,0),0)) 

E  =  FEfcFT  +  Mk 
K  =  EHT(HEHT  +  Nfc)“\ 

where  we  have  dropped  the  subscripts  on  zk+1  and  7/,+  i  for  brevity.  Sinopoli  et  al.  (2004)  present 
the  equivalent  update,  although  derived  by  first  constructing  the  joint  distribution  over  predicted 
state  and  observation  and  then  conditioning  on  the  observation.  Within  the  update  equations,  the 
channel  state  variable  multiplies  the  Kalman  gain,  resulting  in  an  intuitive  behavior — the  standard 
Kalman  update  when  the  measurement  is  present,  and  an  uncorrected  process  prediction  otherwise. 

The  belief  dynamics  defined  within  the  EKF  update  are  random  in  the  current  observation 
and  channel  state,  z  and  7,  respectively.  For  tractable  planning,  we  approximate  the  belief  state 
transition  with  a  Gaussian  transition  model 

bfc+i  =  g{  bfc,  ujfc)  +  W(bfc,  ufc)wfc,  (4.15) 

where  wk  ~  A/”(0, 1).  Since  7  is  not  Gaussian,  we  compute  the  above  expression  by  moment 


(4.13) 

(4.14) 
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matching,  i.e.,  the  first  two  moments  of  (4.15)  are  set  to  that  of  (4.13)-(4.14), 

(4.16) 


(4.17) 


where  E[7]  =  A.  The  evolution  of  beliefs  is  illustrated  in  Fig.  4.2b  and  Fig.  4.2c. 

The  belief  dynamics  formulation  derived  in  Section  4.2.2  follows  a  similar  form,  although 
does  not  consider  random  observation  events.  Without  the  channel  state  variable,  the  portion  of 
the  belief  state  vector  corresponding  to  the  state  covariance  matrix  is  deterministic,  i.e.,  no  random 
variables  appear  in  the  EKF  covariance  update  when  observation  events  are  known.  Here,  the 
covariance  depends  on  7,  and  is  therefore  random.  When  an  observation  is  guaranteed  to  be 
obtained,  however,  A  =  1  and  (4.15)  is  equivalent  to  (4.9). 

The  belief  dynamics  (4.15)  represent  the  predictive  transition  of  a  belief  state  with  random 
observation  events.  Kim  and  Eustice  (2013)  and  Indelman  et  al.  (2015)  both  considered  the  related 
problem  of  planning  with  unknown  loop-closure  events  within  active  SLAM.  Both  parameterize 
Gaussian  beliefs  in  the  information  (inverse  covariance)  form  for  which  the  measurement  update  is 
a  simple  additive  step.  Indelman  et  al.  (2015)  (approximately)  marginalize  over  7  in  the  posterior, 
as  opposed  to  taking  the  expectation  with  respect  to  7,  resulting  in  a  deterministic  information 
(covariance)  matrix.  It  is  also  worth  noting  that  the  belief  dynamics  are  not  invariant  to  belief 
parameterization.  In  the  information  form,  as  in  (Kim  and  Eustice,  2013;  Indelman  et  al.,  2015), 
the  measurement  information  is  scaled  by  A,  whereas,  here,  the  Kalman  gain  is  scaled  instead. 

4.2.4  Belief  Space  Optimization  Motion  Planning 

We  use  the  belief  dynamics  defined  in  (4.15)  to  compute  a  locally  optimal  solution  to  the  cost 
function  defined  by  (4.4).  Previously,  van  den  Berg  et  al.  (2012)  solved  a  similar  objective  using 
dynamic  programming  to  obtain  a  control  policy  mapping  belief  state  to  control  action.  Approach¬ 
ing  the  optimization  as  a  batch  process  instead  allows  us  to  easily  optimize  over  path  parame- 


g(bk,  ufc)  =  E[bfc+i]  =  E 


kfc-fi 

vec(Efc+i) 

/(x*,ufc,0) 
vec(E  -  AKHE) 


r  1 

W(bfc,  Ufc)  =  vVar[bfc+i]  =  , 

Var 

^fc+i 

vec(Efc+i) 

V  AKHE 


\J  A(1  —  A)vvT 


v  =  vec(KHE), 
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Algorithm  5  Belief  space  trajectory  optimization. 

Require:  bi,  U'(IJ  {initial  belief  and  nominal  control} 

1 :  while  not  converged  do 

2:  Afc,  Bfc,  Wfc  =  execute(bj^,  uj^)  for  all  k 

3:  G,  J,  W  =  construct_system({bfc,  Afc,  Bfc,  Wfc}) 

4:  e)U*  =  solve _objective(Q,  R,  G,  J,  W) 

5:  U(4+1)  =  line_search(U^\  5U*) 

6:  end  while 
7:  return  UW 


ters  assuming  a  known  path-following  controller — desirable  for  planning  practical  trajectories  for 
AUVs. 

The  cost  function  (4.4)  can  be  expressed  in  batch  as  a  function  of  the  stacked  vectors  of  beliefs 
and  controls 


min  E  [BtQB  +  UtRU],  (4.18) 

where  we  have  dropped  the  subscripts  on  Bi :L  and  Q  =  diag(Qi, . . . ,  Ql),  and  R  = 

diag(Ri, . . . ,  Rl_i).  We  take  a  sequential  approach — compute  the  set  of  beliefs  as  a  function  of 
control  actions,  substitute  into  the  objective  function,  and  solve  for  the  minimizing  set  of  control 
inputs. 

We  first  linearize  each  belief  transition  around  a  nominal  trajectory  Bi -l  and  control  sequence 

Ul:L-l 


Sbk+1  «  Afcdbfc  +  Bk8uk  +  Wfcwfc,  (4.19) 

where  db/,:  =  b^  —  b^,  =  u^-  —  U/c,  and  Ak  and  are  the  Jacobians  of  g(bk,  u^)  with  respect 

to  hk  and  uk,  respectively.  Note  that  B1:i  is  obtained  by  executing  g{  ■ )  along  the  nominal  control 
trajectory  given  bi.  We  also  linearize  W(b^,  u/c)  such  that  the  7th  column  can  be  written 

W<°  «  F^8bk  +  G®6uk  +  W<°  (4.20) 

and  Wj.0  =  WW(bfc,  ufc).  Note  that  Wk  is  a  function  of  the  channel  model  with  parameter  Xk.  In 
Section  4.4  we  discuss  the  form  of  Xk,  which  depends  on  the  state.  During  planning,  we  evaluate 
Xk  about  the  nominal  trajectory. 
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We  can  write  the  stacked  vectors  of  belief  state  and  control  deviations  as 


6B  =  [5bJ,...,SbTL]T  (4.21) 

5U=  [<Ju7,...^uI_1]T.  (4.22) 

The  batch  belief  dynamics  are  then  expressed  by  concatenating  (4.19)  over  the  time  horizon 


5B  =  A5B  +  BTU  +  Ww 

SB  =  (I  -  A)-1B5U  +  (I  -  A)_1W  w,  (4.23) 

G  J 

where  w  is  the  stacked  vector  of  noise  perturbations  wk  and  the  stacked  Jacobians  are  defined 


A  = 


0 

A,  0 
0 

0  A  L-1  0 


B 


0 

B1  0 
0  '• 


0  Bl-i 


w  = 


0 

Wi  0 
0 


0  WL_! 


(4.24) 


(4.25) 


(4.26) 


We  substitute  the  batch  belief  dynamics  (4.23)  into  the  cost  function  (4.18)  and  evaluate  the 
expectation  to  obtain 

min  [(G5U  +  B)tQ(G<5U  +  B)  +  (<5U  +  U)TR(5U  +  U)  +  tr(JTQJ)l 
=  min  [(G<5U  +  B)TQ(G5U  +  B)  +  (5U  +  U)TR(5U  +  U) 

+  E,;((F('°G  +  GW)<JU  +  W«)T(I  -  A)-TQ(I  -  A)-1((F«G  +  G«)TU  +  W«)]. 


This  cost  function  is  quadratic  in  the  control  update  term,  <5U,  and  can  therefore  be  minimized  by 
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taking  the  derivative  of  the  cost  and  setting  to  zero 


5U*  =  — D_1E, 


(4.27) 


where 


D  =  GtQG  +  R  +  Et(F(i)G  +  G«)TC(F«G  +  G«) 

E  =  GTQ(iB  +  JA(F«G  +  G«)TCW« 

C  =  (I-A)-tQ(I-A)~1. 

We  leverage  the  sparsity  structure  of  to  efficiently  compute  the  summation  terms.  Note  that 
the  Hessian  is  positive  definite  by  construction  because  of  the  cost  weight  matrices.  The  updated 
control  vector  is  computed  by  adding  the  update  to  the  control  at  the  current  iteration 

Tj(f+1)  =  Tj(f)  +  ear,  (4.28) 

where  e  defines  the  step  length  based  on  a  simple  back-tracking  line  search.  The  control  update  step 
is  repeated  until  a  convergence  criteria  is  satisfied.  The  full  algorithm  is  outlined  in  Algorithm  5. 

4.2.5  Parameterized  Trajectory  Optimization 

In  the  previous  section,  we  described  a  method  to  compute  a  sequence  of  open- loop  control  actions 
over  a  finite  horizon.  He  et  al.  (2011)  proposed  planning  over  macro  actions  (a  collection  of 
sequential  actions)  to  reduce  planning  complexity,  and  allow  planning  over  a  longer  time  horizon. 
Planning  over  path  parameters,  similar  to  macro  actions,  has  also  been  studied  by  both  Sim  et  al. 
(2004)  and  Kollar  and  Roy  (2008).  Here,  we  apply  an  optimization  over  parameterized  paths  for 
planning. 

For  AUV  field  trials,  we  desire  simple,  practical  trajectories  in  order  to  easily  monitor  vehi¬ 
cle  health  and  progress.  To  address  this,  we  assume  that  each  vehicle  employs  a  feedback  path¬ 
following  controller  and  optimize  over  parameters  that  define  a  trajectory.  For  example,  a  center 
position  and  radius  define  a  circular  path.  The  control  is  then  a  (known)  function  of  the  estimated 
vehicle  state  and  path  parameter  vector  6.  The  state  transition  model  (4.1)  can  be  expressed  given 
the  controller  and  parameter  vector  as 


Xfc+l  =  /(xfc,  Ufc(xfc,0),Wfc). 


We  iteratively  update  locally  optimal  path  parameters  in  the  same  way  control  actions  were 
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computed.  In  this  case,  Jacobians  are  computed  with  respect  to  the  path  parameters  instead  of 
control  actions.  Finally,  we  penalize  parameter  weights  (instead  of  control)  within  the  cost  function 
to  ensure  a  positive  definite  Hessian.  The  number  of  parameters  to  specify  a  path  is,  in  general, 
much  smaller  than  the  number  of  control  actions.  Therefore,  there  is  also  significant  computational 
savings  in  optimizing  over  path  parameters. 


4.3  Planning  for  Cooperative  Localization 


Here,  we  detail  the  instantiation  of  the  belief  space  planning  algorithm  developed  in  Section  4.2  to 
plan  a  server  trajectory  to  localize  a  client  vehicle.  We  require  that  the  nominal  client  trajectory  and 
path-following  controller  are  available  at  planning  time.  We  further  assume  that  the  client  vehicle 
is  able  to  reconstruct  the  centralized  server-client  estimator,  as  with  the  OSM  (Chapter  2),  and  that 
communication  latency  is  negligible. 

The  state  space  system  is  represented  by  the  stacked  server  and  client  vehicle  states  = 
[xj  ,  xJfc]T.  The  state  space  dynamics  of  each  vehicle  are  independent  and  written 


XSfc  +  l 

_XCfc  +  l_ 

/.s(X 

/c(x 


Ski 

Ckl 


Wfc,wSfe) 

Ucfc,WCfc)_ 


(4.29) 


where  uSfe  represents  the  only  decision  variable  control  action  within  the  planning  problem  since 
uCj,  is  computed  by  the  client’s  path-following  controller  and  a  given  mission  plan. 

Relative  range  constraints  constitute  a  nonlinear  observation  over  the  server-client  state 


°rk 


+  W 


rki 


(4.30) 


where  wrk  r->_/  Af(  0  ,  ) .  In  general,  range  observations  will  be  a  3D  slant  range  measurement  (see 
Appendix  B.2  for  more  details).  Within  a  linear  estimator,  such  as  the  EKF,  range  observations 
add  information  in  one  direction:  along  the  vector  between  the  server  and  client  vehicle  positions. 
For  underwater  navigation,  depth  is  usually  well  instrumented,  so  we  seek  to  minimize  horizontal 
position  errors.  Therefore,  the  range  measurement  utility  is  highest  when  the  server  and  client  ve¬ 
hicles  are  far  apart,  i.e.,  the  vector  of  added  information  is  approximately  parallel  to  the  horizontal 
plane. 

In  all  experiments  presented  in  this  chapter,  we  use  a  cost  function  that  penalizes  control  action 
and  client  vehicle  uncertainty.  Unlike  many  planning  problems,  there  is  no  desired  terminal  state 
for  the  server.  We  can  define  a  quadratic  stage  cost  penalizing  client  uncertainty  fitting  the  form  of 
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Figure  4.3  Reception  rate  data  and  channel  state  model.  The  histogram  represents  the  frequency  of  successfully 
received  acoustic  broadcasts.  The  channel  model.  A,  illustrates  (4.31)  tuned  to  the  environment. 


the  objective  in  (4.4): 


tr(£cJ  =  mbfc 
tr2(ScJ  =  b^mmbfc 

=  bjQb  k, 

where  mT  is  a  row  vector  that  sums  the  diagonal  elements  of  the  covariance  matrix  corresponding 
to  the  client  vehicle  from  the  belief  state. 


4.4  Numerical  Simulations 

We  first  present  an  empirical  channel  model  informed  by  reception  statistics  collected  during  AUV 
field  trials.  We  then  validate  our  planning  framework  through  several  simulated  scenarios  where 
we  employ  this  channel  model  for  planning. 


4.4.1  Empirical  Channel  Model 


In  this  section,  we  review  how  we  compute  the  distribution  over  channel  states  [>{"ik  =  1)  = 
Afc.  Physics-based  and  empirical  models  exist  for  studying  communication  error  in  underwater 
channels.  One  such  empirical  channel  model  proposed  by  Stojanovic  (2007)  was  used  by  Hollinger 
et  al.  (2015)  for  studying  multiple  vehicle  coordination.  Within  the  model,  the  probability  of 
successful  transmission  is  written  as  a  function  of  the  transmit  frequency,  power,  and  environmental 
conditions.  In  this  chapter,  we  are  interested  in  showing  how  a  channel  model  can  be  integrated 
into  the  planning  framework.  As  such,  we  employed  a  simplified  model  where  the  probability  of 


successful  reception  falls  off  as  a  sigmoid  with  the  horizontal  range,  r  = 


,  between 
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server  and  client 


Afc(r)  =  pi^k  =  1) 

(4.31) 

where  f  is  the  range  at  which  reception  probability  is  \  and  r  is  the  length  scale  that  defines  the 
rate  at  which  reception  decreases. 

We  manually  fit  the  parameters  of  this  simple  model  to  data  collected  over  the  course  of  sev¬ 
eral  field  trials.  We  fielded  two  Iver2  AUVs  and  a  surface  ship.  Each  vehicle  was  equipped  with 
a  WHOI  Micro-modem  and  co-processor  board  (Freitag  et  al.,  2005b),  25  kHz  BTech  Acous¬ 
tics  2RCL  transducer,  and  a  synchronous-clock  reference.  Each  vehicle  periodically  transmitted 
Micro-modem  Rate  1  and  2  data  packets  which  each  consist  of  three  64  B  PSK  encoded  data 
frames.  The  vehicle  hardware  and  software  systems  are  reviewed  in  Appendix  C. 

Over  the  course  of  three  trials,  we  collected  over  3000  range  observations.  The  trials  were 
conducted  in  shallow  water  (roughly  20  m)  with  all  transducers  suspended  at  approximately  10  m. 
While  the  true  inter- vehicle  range  is  unknown,  we  estimated  range  from  the  filtered  online  vehicle 
position  estimates  in  post  process.  The  inter-vehicle  range  did  not  exceed  400  m.  Fig.  4.3  shows 
the  channel  model  (4.31)  overlaid  on  a  histogram  of  reception  frequencies  for  varying  relative 
range,  where  the  channel  model  matches  the  characteristic  behavior  of  the  true  channel. 

4.4.2  Simulation:  Static  Beacon  Localization 

Our  first  set  of  simulations  helps  illustrate  the  planning  problem  for  a  simple  scenario — compute 
a  server  trajectory  to  localize  a  static  beacon.  This  is  a  special  case  of  server-client  localization 
in  which  the  client  vehicle  is  fixed,  and,  hence,  does  not  accrue  any  additional  uncertainty  over 
time.  This  problem  is  akin  to  surveying  LBL  network  beacons.  Jakuba  et  al.  (2008)  recommend 
that  the  topside  ship  circle  the  beacons  at  a  fixed  radius  (up  to  water  depth),  providing  a  heuristic 
to  balance  position  information  and  communication. 

We  assume  that  the  server  vehicle  follows  a  simple  unicycle  model.  The  server  translates  a 
fixed  distance,  A,  for  each  timestep  with  control  defined  by  a  change  in  heading,  uk,  perturbed  by 
zero-mean  independent  Gaussian  noise  Wk ■  The  server  state  is  parameterized  by  its  XY  horizontal 
position  and  heading  9: 


1  +  exp  <  —  (r  —  r) 
r 


•Gfc+i 

xSk  +  A  cos (9Sk  +  uk  +  wk) 

Vsk+i 

= 

ySk  +  A  sin(0Sfe  +  uk  +  wk ) 

n 

_usk+ 1_ 

9Sk  +  Uk  +  wk 
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Figure  4.4  Example  planning  for  static  beacon  localization.  Top  row  shows  planned  trajectories  with  the  underlying 
channel  model  using  the  baseline  relative  depth  (color  gradient  in  top  row  represents  communication  channel  quality, 
darker  is  better).  Bottom  row  plots  range  information  using  the  baseline  channel  model  (color  gradient  in  bottom  row 
represents  range  information,  darker  is  better). 


(a)  Baseline  scenario  with  channel  (b)  More  restrictive  communication  (c)  Less  restrictive  communication 
model  channel  channel 


The  beacon  (client)  state  includes  simply  its  XY  position.  Following  each  motion  step,  the  server 
vehicle  observes  its  own  position  and  orientation  and  the  range  to  the  beacon  (depending  on  the 
channel  state). 

We  optimized  over  open-loop  control  actions  and  parameterized  circular  paths.  For  the  opti¬ 
mal  control  trajectories,  the  server  vehicle  was  initialized  in  the  same  position  for  each  trial.  For 
the  parametric  circular  trajectories,  the  initial  server  pose  is  a  function  of  the  radius.  Fig.  4.4  il¬ 
lustrates  planned  server  trajectories  for  varying  channel  model  and  relative  depth  parameters  over 
50  planning  steps.  This  scenario  illustrates  the  tradeoff  between  a  good  communication  channel 
or  informative  range  observations.  This  tradeoff  is  endemic  to  all  range-only  cooperative  under¬ 
water  planning.  The  top  row  in  Fig.  4.4  demonstrates  that  our  planning  framework  responds  to 
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varying  channel  parameters.  The  bottom  row  shows  that  information  added  by  each  range  mea¬ 
surement  changes  as  a  function  of  relative  depth  between  the  server  and  beacon.  As  the  relative 
depth  increases,  the  server  must  move  farther  away  in  order  to  localize  the  beacon. 

These  simulations  also  help  to  illustrate  the  utility  of  parameterized  trajectories,  provided  that 
the  parameters  are  well  informed  by  experience.  For  this  simplified  scenario,  the  planned  control 
and  parameterized  circle  paths  do  not  differ  significantly  and  agree  with  heuristic  LBL  localization 
methods. 

4.4.3  Simulation:  Server-Client  Localization 

In  the  second  set  of  simulated  trials,  we  consider  two  mobile  AUVs — a  server  and  client — and 
show  that  the  planning  framework  scales  to  realistic  problem  sizes.  Each  vehicle’s  state  is  parame¬ 
terized  by  its  XY  horizontal  position  and  heading  6.  We  assume  a  constant  forward  velocity  so  that 
over  a  fixed  time  step,  the  change  in  position  is  A.  The  state  dynamics  follow  a  simplified  bicycle 
model  with  steering  angle  control  input 


XSk+ 1 

xsk  +  (A  +  wAfc)cos(0Sfc) 

Vsk+1 

= 

Vsk  +  (A  +  wAfc)sm(0Sfc) 

_@sk+ i_ 

dSk  +  a(A  +  wAJ  tan(ufc  +  wUk)_ 

where  a  is  the  reciprocal  ‘wheel-base’  length  and  wAfc  and  wUk  represent  Gaussian  noise  perturba¬ 
tions  in  the  step  length  and  control,  respectively. 

We  are  provided  the  client  survey  trajectory  at  planning  time.  The  estimated  survey  time  is 
76  min  and  is  represented  by  roughly  5000  state  transitions.  The  relative  vehicle  depth  is  100  m 
and  the  channel  model  parameters  are  f  =  250  m  and  r  =  125  m.  By  optimizing  over  ‘diamond’ 
path  parameters  (center  position,  width,  height),  we  reduce  the  complexity  of  the  problem.  Each 
vehicle  relies  on  a  pure-pursuit  path-following  controller.  Fig.  4.5  depicts  the  planned  trajectory 
for  each  vehicle. 

We  also  compared  the  optimized  path  to  a  naively  planned  path,  which  we  have  employed  in 
several  previous  cooperative  localization  field  trials.  We  set  the  parameters  of  the  naive  path  to  be 
centered  on  the  client  survey  area  and  span  the  length  and  width  of  the  survey  area.  Table  4.1  sum¬ 
marizes  the  performance  of  each  path  averaged  over  100  simulated  trials.  Overall,  the  optimized 
path  receives  more  OWTTs  per  trial  than  the  naively  planned  trajectory.  Moreover,  the  client  un¬ 
certainty  (indicated  by  the  determinant  and  trace  in  the  table)  is  far  lower  for  the  optimized  path. 
The  optimized  server  path  remains  closer  to  the  center  of  the  client  survey,  and  thus  is  able  to  more 
reliably  make  OWTT  observations. 
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Figure  4.5  Optimized  (red)  and  naively  planned  (green)  server  trajectories. 


Table  4.1  Average  performance  comparing  an  optimized  path  and  a  naively  planned  path  over  100  simulated  trials. 


Method 

OWTTS  RECEIVED 

|EcJ  tr(EcJ 

Proposed  (optimized) 

67.6 

0.40  69.56 

Naively  planned 

62.9 

1.60  402.34 

4.5  Chapter  Summary 

Cooperative  underwater  localization  enables  teams  of  vehicles  to  exploit  range  observations  to  im¬ 
prove  their  navigation  estimates.  The  quality  of  the  estimate,  however,  depends  on  the  relative 
position  of  the  vehicles.  Therefore,  planning  relative  paths  can  benefit  online  navigation  perfor¬ 
mance.  We  have  presented  a  probabilistic  channel  model  that  represents  randomness  in  observation 
events.  The  channel  model  holds  potential  utility  in  other  applications  involving  unknown  mea¬ 
surement  acquisition.  We  then  integrated  the  model  into  a  belief  space  planning  framework  that 
optimizes  either  open-loop  control  actions  or  path  parameters  in  order  to  produce  practical  AUV 
trajectories. 


91 


Chapter  5 

Conclusions  and  Future  Work 


AUVs  extend  the  abilities  of  ocean  researchers  to  observe  the  world  in  a  way  not  previously 
possible — at  greater  depths  and  over  larger  spatial  and  time  scales.  Accurate  underwater  local¬ 
ization  is  a  key  competency  for  safe  operation  and  to  precisely  geo-reference  scientific  data.  Co¬ 
operative  localization  has  recently  been  enabled  by  more  prevalent  underwater  communication 
systems,  but  faces  several  design  challenges  in  transitioning  to  the  underwater  domain. 

Underwater  cooperative  localization  provides  an  infrastructure-free  means  of  accurate  bounded- 
error  position  estimation  while  subsea.  The  design  of  such  systems  must  address  how  to  share  in¬ 
formation  consistently  and  how  to  communicate  such  information  over  a  lossy  and  low-bandwidth 
underwater  acoustic  communication  channel.  Prior  to  this  thesis,  estimator  consistency  had  been 
addressed  by  expanding  the  state  representation  to  explicitly  track  correlated  information,  i.e.,  by 
using  delayed  state  or  pose-graph  models.  While  providing  a  solution  to  the  problem  of  consis¬ 
tency,  a  larger  state  representation  did  not  necessarily  lend  itself  to  communication  over  a  band¬ 
width  limited  channel.  Additionally,  the  estimate  quality  produced  by  range-only  cooperative  lo¬ 
calization  is  strongly  influenced  by  the  network  geometry.  No  previous  literature  has  addressed 
planning  informative  relative  trajectories  that  considers  both  the  quality  of  information  and  the 
fidelity  of  the  acoustic  channel. 

5.1  Contributions 

The  specific  contributions  of  this  thesis  include: 

•  We  proposed  the  OSM  (Chapter  2)  for  cooperative  server-client  localization.  The  DEIF, 
which  noted  that  local  platform  information  updates  take  a  sparse  form  in  a  pose-graph 
framework,  served  as  the  basis  for  this  work.  We  showed  that  we  could  further  exploit  the 
structure  of  the  underlying  pose-graph  in  order  to  broadcast  the  server  information  to  the 
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client.  We  also  provided  experimental  evaluation  for  the  first-ever  real-time  exact  OWTT 
cooperative  localization. 

•  In  Chapter  3,  we  presented  an  algorithm  for  peer-to-peer  cooperative  localization.  We  bor¬ 
rowed  ideas  from  the  OSM  in  order  to  effectively  share  local  information  that  scales  to  many 
vehicles.  While  the  OSM  leveraged  the  structure  of  the  MRF,  here,  we  exploited  the  factor- 
level  structure  of  the  local  information.  We  showed  that  odometry  factors  can  be  composed 
and  then  decomposed  for  efficient  communication.  We  also  applied  approximate  marginal¬ 
ization  to  reduce  the  number  of  pose  nodes  that  must  be  shared.  We  provided  a  comparative 
experimental  evaluation  that  validated  our  approach  and  showed  improvement  when  com¬ 
pared  to  a  similar  method. 

•  We  proposed  an  motion  planning  algorithm  for  server-client  localization  that  includes  a 
probabilistic  channel  model.  The  algorithm  is  based  on  prior  work  in  belief  space  plan¬ 
ning  methods,  which  provide  a  solution  to  more  general  POMDPs.  Solving  the  POMDP  is 
made  tractable  by  applying  linear  Gaussian  techniques.  We  model  acoustic  communication 
success  as  a  spatially-varying  Bernoulli  random  variable,  termed  the  channel  state  variable. 
We  modify  the  measurement  observation  model  to  include  the  channel  state  variable,  and 
then  derive  the  corresponding  EKF  for  such  a  model.  The  EKF  propagation  equations  define 
the  so-called  belief  dynamics,  which  provides  a  predictive  model  of  client  state  and  uncer¬ 
tainty  given  a  server  path.  Finally,  we  optimize  over  server  path  parameters  to  obtain  locally 
optimally  server  paths. 


5.2  Future  Work 

While  this  thesis  has  provided  novel  contributions  toward  underwater  cooperative  localization,  we 
recognize  room  for  improvement.  Further  compelling  areas  of  research  are  described  below. 

Cooperative  Localization 

•  The  methodologies  reported  in  this  thesis  enable  platforms  to  share  their  local  navigation  in¬ 
formation.  However,  there  is  no  mechanism  for  sharing  information  from  the  wider  network, 
which  would  be  useful  for  cascaded  communication  topologies,  for  example.  For  cascaded 
network  topologies  (or  any  tree  topology),  we  propose  approximate  marginalization  (as  in 
Chapter  3)  to  include  parent  information  as  an  additional  factor  for  children  platforms. 

•  We  have  only  focused  on  handling  direct  vehicle-to-vehicle  range  constraints  as  derived 
from  OWTT  observation.  Based  on  our  factor-based  localization  framework,  vehicles  could 
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potentially  share  additional  loop-closure  or  other  informative  factors,  e.g.,  from  a  visual 
SLAM  system. 

•  Roumeliotis  and  Rekleitis  (2004)  examined  theoretical  bounds  on  uncertainty  growth  within 
cooperative  networks  as  a  function  of  network  size.  Their  analysis  places  strong  assumptions 
on  the  sensors  available  to  each  vehicle  and  only  considers  unconstrained  communication. 
As  future  work,  we  propose  extending  their  analysis  to  graph-based  cooperative  estimation 
frameworks  with  unreliable  communication  as  in  Chapter  3.  This  analysis  would  indicate  ex¬ 
pected  localization  performance  for  underwater  vehicle  cooperative  networks,  which  could 
be  useful  for  multi-vehicle  survey  design. 

Planning  for  Localization 

•  Optimizing  over  path  parameters  is  similar  to  using  macro  actions  (He  et  al.,  2011).  It 
would  be  interesting  to  explore  a  richer  set  of  actions  (only  diamond  trajectories  employed 
in  Chapter  4)  that  could  be  sequentially  combined,  yet  still  easily  computed. 

•  Our  planning  methodology  addressed  planning  for  a  single  server  vehicle  given  a  single 
client  trajectory.  A  possible  avenue  for  future  work  would  be  to  simultaneously  consider 
multiple  server  and  client  vehicles.  Scaling  to  multiple  vehicles  is  nontrivial  since  planning 
algorithm  complexity  is  super  linear  in  the  number  of  vehicles. 

•  Informative  planning  for  range-only  localization  can  be  accomplished  using  heuristic  plan¬ 
ners,  e.g.,  German  et  al.  (2012)  proposed  following  the  centroid  of  the  client’s  remaining 
survey  area.  These  heuristic  planners  can  perform  quite  well  in  practice  by  capturing  the  in¬ 
tuitive  notion  that  localization  quality  is  tied  to  varying  the  relative  angle  between  vehicles — 
in  order  to  bound  uncertainty  in  all  directions — and  were  designed,  no  doubt,  from  user  ex¬ 
perience.  In  the  same  vein,  learning  algorithms  may  be  capable  of  selecting  good  support 
trajectories  given  past  performance.  A  learned  behavior  may  have  computational  benefits 
that  enable  rapid  replanning  or  may  scale  to  many  vehicles. 
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Appendix  A 

Probabilistic  Graphical  Models 


The  connection  between  solutions  to  linear  systems  and  graphical  models  has  been  long  studied 
(Parter,  1961;  Rose,  1972;  Kschischang  et  al.,  2001;  Loeliger,  2004;  Cowell  et  al.,  2007)  and  helps 
to  reveal  the  underlying  structure  in  the  SLAM  problem.  Fig.  1.2  illustrates  the  connection  between 
the  information  matrix  and  the  Markov  random  field  (MRF)  and  the  measurement  Jacobian  and  the 
factor  graph.  Understanding  the  relationship  between  the  SLAM  problem  and  graph  representation 
has  led  to  the  development  of  the  current  state-of-the-art. 

Probabilistic  graphical  models  capture  conditional  independence  relationships  within  the  joint 
distribution  over  a  set  of  random  variables.  The  joint  distribution  is  often  more  simply  expressed 
as  a  product  over  conditionally  independent  subsets.  A  random  variable  A  is  conditionally  inde¬ 
pendent  of  C  given  B,  i.e.,  A  _LL  C  \  B,  if  p(A  \  B,C)  =  p(A  \  B ).  Below,  we  provide  a  brief 
overview  of  the  two  graphical  models  with  Gaussian  noise  models  referenced  in  this  work. 

A.l  Gaussian  Markov  Random  Fields 

An  MRF  is  a  multivariate  random  variable  X  =  {xi, . . . ,  x„  }  that  can  be  represented  by  an  undi¬ 
rected  graphical  model  G  =  ( V. ,  E)  where  V  and  E  are  the  vertex  (node)  and  edge  sets,  respec¬ 
tively.  Two  unique  nodes  x,  and  xy  for  i,j  <G  V  are  adjacent  if  they  share  an  edge,  {i,  j}  G  E.  The 
neigbors  of  x,:  are  the  nodes  with  which  x*  is  adjacent,  i.e.,  Af(i)  =  {xj\{i,j}  e  E,Vj  e  V}. 


Figure  A.l  Illustration  of  a  Gaussian  MRF.  The  information  matrix  A  corresponds  exactly  to  the  adjacency  matrix  of 
the  graph. 

% 

A 

O — C^—Q—O 
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Graph  edges  encode  conditional  relationships  within  the  distribution — a  variable  node  is  inde¬ 
pendent  of  the  rest  of  the  graph  given  its  neighbors, 


P(Xi|xi6Af(i))- 


A  special  case  of  MRFs  are  graphs  that  admit  a  clique  (complete  subgraph)  factorization,  i.e.,  the 
joint  density  can  be  factored 


p(x)= n^(xc), 

cec 


(A.l) 


where  (/>(■)  is  termed  a  factor  potential. 

A  multivariate  Gaussian  distribution  is  parameterized  by  its  mean  and  covariance,  or,  in  the 
natural  form,  by  its  information  vector  and  matrix 

X  ~  E)  =AT-1(r/,A). 

The  information  matrix,  A,  is  exactly  the  adjacency  matrix  of  G  corresponding  to  the  MRF.  More 
specifically,  off-diagonal  block  entries  of  the  information  matrix  are  zero  for  pairs  of  vertices  that 
do  not  share  an  edge 


{i,j}  E  =>  A ij  =  0.  (A. 2) 

This  property  is  illustrated  in  Fig.  A.l. 

A.2  Factor  Graphs 

The  factor  graph  is  a  bipartite  graphical  model  consisting  of  variable  nodes  and  factor  nodes. 
Within  the  SLAM  problem,  factors  or  constraints  typically  represent  observations  connecting  un¬ 
known  pose  or  landmark  variables.  Like  the  MRF,  the  factor  graph  reveals  the  underlying  factor- 


Figure  A.2  Illustration  of  a  Gaussian  graph.  The  measurement  Jacobian  A  directly  corresponds  to  the  factor  graph. 
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ization  of  the  joint  distribution 


p(x)«n*(*) 

i 


(A. 3) 


where  fa  ( • )  represents  the  the  potential  induced  by  the  zth  factor  node  over  the  set  of  variable 
nodes,  X,:. 

The  factor  graph  shares  a  direct  correspondence  with  the  measurement  Jacobian — each  factor 
corresponds  to  block  row  within  the  measurement  Jacobian. 
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Appendix  B 

Vehicle  and  Observation  Models 


In  this  appendix,  we  review  the  2D  horizontal-plane  odometry  model  used  in  the  experiments 
presented  in  this  dissertation.  We  also  describe  and  discuss  the  use  of  2D  pseudo  range  observations 
derived  from  3D  slant  range  observations. 


B.l  DVL  Odometry  Model 


In  this  section,  we  outline  the  procedure  to  compute  odometry  updates  given  noisy  DVL  and  head¬ 
ing  data.  We  employ  a  first-order  covariance  approximation  as  developed  by  Eustice  et  al.  (2011). 

The  vehicle  horizontal-plane  odometry  (displacement)  in  the  world  frame  between  time  indices 
i  and  j  is  computed 


A  x.y  =  /  R(,0(f))u  (t)dt, 


(B.l) 


where  u (f)  =  [u(t),  v(t)}'  is  the  vector  body-frame  velocity  (surge  and  sway),  wit)  is  the  vehicle 
heading,  and  negligible  roll  and  pitch  is  assumed.  The  body  to  world  rotation  matrix  is  defined 


m 


cos  6  —  sin  9 
sin  6  cos  6 


(B.2) 


We  can  approximate  (B.l)  given  discrete  heading  and  velocity  samples  by  forward  Euler  numerical 
integration 


Ax,j  =  At  RC0fc)ufc, 

k 


(B.3) 


with  sample  index  k  and  sample  period  At. 
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DVL  velocity  and  heading  observations  are  nondeterministic.  We  model  the  random  noise 
perturbations  by  independent  additive  zero-mean  Gaussian  noise 

zUfc  =  Ufc  +  wUk,  wUk  ~  J\f( 0,  RUfc) 

Z^k  =  0 k  +  W^k ,  w^k  ~  Af (0,  a\ J . 

The  observed  odometry  is  then  computed  as  the  summation  of  noisy  heading  and  velocity  mea¬ 
surements 


zij  At  ^  ^  R(z^pk)zUk 

k 

=  Af  y^R(-0fc  +  wv,J(ufc  +  wuJ.  (B  .4) 

k 

To  approximate  odometry  observation  statistics,  we  first  compute  the  first-order  Taylor  series 
expansion  of  the  rotation  matrix  in  (B.4) 


R(^fe  +  utyj  ~ 


cos  0 k  -  w^k  sin  i>k  -  sin  0 fc  -  w^k  cos  0fc 
sin  0fc  +  cos  0fc  cos  0fc  -  w^k  sin  0fc 


cos  —  sin 
sin  0*.  cos  0*. 

R(VA 


sin  cos 
—  cos  0*.  sin  0*. 


R'(V’fc) 


(B.5) 


We  then  substitute  (B.5)  into  (B.4) 


z?y  ~  At  ^  (R(0fe)  -  utyfcR'(0fc))  (ufc  +  wuJ  .  (B.6) 

k 

Finally,  the  approximate  odometry  observations  statistics  are  determined 

E[z  ij\  =  At  R(0fc)ufc 

k 

=  A  xi:j  (B.7) 

R ij  =  E  [(z ij  -  E[zij])(zij  -  E[zjj])T] 

=  At2  (R(0fe)RUfcR(0fe)T  +  R'(0k)  (wuJ  +  RUfc)  R/(,0fc)T)  (B.8) 

k 


where  R,y  is  the  approximate  odometry  noise  covariance. 
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Figure  B.l  Posterior  distribution  over  client  state  given  a  single  range  to  a  beacon  at  the  origin  and  relative  depth.  The 
horizontal  range  (green  circle)  is  held  constant  while  the  relative  depth  is  increased  from  left  to  right.  Note  that  the 
distribution  becomes  more  uncertain  (less  informative)  as  relative  depth  increases. 


(a)  Posterior  distribution  over  state  given  3D  slant  range  and  depth  observations. 


B.2  OWTT  Observation  Model 


Many  estimation  and  planning  frameworks  for  OWTT  localization  project  3D  slant  ranges  into  the 
2D  horizontal  plane  to  obtain  a  set  of  pseudo  ranges  used  with  a  fixed  measurement  covariance. 
Here,  we  give  a  formulation  for  the  pseudo  range  as  a  function  of  the  measured  slant  range  and 
relative  depth  between  the  server  and  client  and  provide  a  first-order  covariance  approximation. 
Fig.  B.l  illustrates  that  increasing  the  relative  depth  between  vehicles  reduces  the  horizontal  plane 
information. 

We  model  a  slant  range  between  3D  client  and  server  states  xc,  xs  e  M3  as 


-slant 


2 


T  Aslant 


(B.9) 


where  ws iant  ~  A^(0,  oflant).  We  assume  that  we  have  access  to  a  relative  depth  observation 


zd  =  zs  -  zc  +  wd, 


where  zs  and  zc  are  the  depth  components  of  x,s  and  xc,  respectively,  and  wd  ~  A/"(0,  of/) .  Slant 
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Figure  B.2  Pseudo  range  variance  (7pSeudo  and  information  1  / <7pSelldo  for  increasing  depth  to  range  ratio.  As  the  relative 
depth  increases  relative  to  the  slant  range,  the  variance  increases  corresponding  to  a  decrease  in  information  in  the 
horizontal  plane. 


0.0  0.2  0.4  0.6  0.8  1.0 


depth  to  range  ratio,  zd/zalant 


ranges  are  projected  into  the  horizontal  plane  given  the  relative  depth 


_(  2  _  A  1/2 

^pseudo  —  \Alant  Zd ) 

=  ((||xa  -Xc||2  +MW)' 


for  Zd  <  ^siant-  We  can  model  zpscudo  as  a  Gaussian  random  variable  with  variance  approximated 
through  the  first-order  Taylor  expansion,  i.e. 


2  _  ^slantu  slant  '  ^du  d 

°pseudo  —  „2  _  '2 

*  slant  *  d 


(B.10) 


The  variance  is  not  a  simple  weighted  combination  of  the  depth  and  slant  range  variances,  nor 
is  it  a  fixed  value.  The  variance  explodes  as  zd  — >  zsiant,  regardless  of  the  depth  and  slant  range 
uncertainties.  Linearized  range  observations  add  information  along  the  vector  between  the  server 
and  client  positions.  As  zd  — >  zs iant,  almost  all  information  is  added  along  the  depth  direction,  and 
little  information  is  added  in  the  horizontal  plane.  This  relationship  is  illustrated  in  Fig.  B.2. 
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Appendix  C 


Field  Equipment 


C.1  PeRL  Iver  AUVs 

The  Perceptual  Robotics  Laboratory  (PeRL)  maintains  two  AUVs — modified  Ocean-Server,  Inc. 
Iver2  AUVs,  Iver28  and  Iver31.  Fig.  C.l  showes  a  schematic  overview  of  the  custom  Iver2  con¬ 
figuration. 

C.1.1  Hardware  description 

Each  AUV  includes  a  typical  advanced  dead-reckoning  sensor  suite  as  tabulated  in  Table  C.l  and 
reported  in  Brown  et  al.  (2008,  2009).  The  AUVs  measure  body-frame  velocities  with  a  600  kHz 
RDI  DVL,  attitude  with  a  Microstrain  3DM-GX25-AHRS,  and  depth  with  a  Desert  Star  Systems 
SSP-1  digital  pressure  sensor.  Additionally,  each  vehicle  is  outfitted  wit  a  WHOI  Micro-modem 
and  co-processor  board  Freitag  et  al.  (2005b)  and  25  kHz  BTech  Acoustics  2RCL  transducer. 

To  calibrate  the  Microstrain  heading  bias,  we  apply  a  nonlinear  bias  correction  to  the  raw 
magnetic  compass  reading  6, 

66(6)  =  A  +  B  sin  6  +  C  cos  6  +  D  sin  26  +  E  cos  26. 


Table  C.l  Iver  Navigation  sensors:  Sampling  frequency  and  noise  characteristics 


Iver  Sensors 

State  Variable 

Frequency 

Noise  (l-cr) 

Micro  strain 

25  Hz 

2° 

DVL 

i,y 

3  Hz 

5  cm/s 

pressure  depth 

z 

2  Hz 

10  cm 

acoustic  modem 

slant  range 

every  ~30  sec 

1  m 

GPS 

x,y 

1  Hz 

3  m 

LBL 

x,y 

every  ~30  sec 

1  m 
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Figure  C.l  Schematic  Iver2  configuration. 

Animatics  SM2315-DT 
Motor 


GPS 

802.11  Wi-Fi  ^ 


KVH  DSP-3000  (FOG) 


BTech  Acomms 

Applied  Acoustics  Transducer 
USBL 


Desert  Star  SSP-1 
Depth  Sensor 


Microstrain 
3D-GX1  AHRS 


Applied  Acoustics  USBL 
RDl  Explorer  600kHz  DVL 


Prosilica  GC1380H(C) 

1 2-bit  Stereo  Cameras 


The  bias  correction  is  a  simple  sum  of  harmonic  functions  fit  during  a  special  calibration  mission. 

C.1.2  Software  description 

All  vehicles  run  a  robotic  operating  system,  peris,  developed  by  PeRL.  The  modular  and  ex¬ 
tensible  software  architecture  allows  vehicles  to  log  command,  control,  and  sensor  data  real-time 
and  then  later  playback  for  new  development  and  debugging.  The  operating  system  also  contains 
myriad  tools  for  real-time  visualization  and  control.  Bingham  et  al.  (201 1)  provide  further  details. 

C.2  Synchronous-Clock  Acoustic  Communication 

A  pulse  per  second  (PPS)  pulse  is  used  to  trigger  acoustic  transmission  events  with  the  WHOI 
Micro-modem  (Freitag  et  al.,  2005b).  Subsea,  a  PPSBoard  (Eustice  et  al.,  2006b)  provides  a  stable 
precision  clock  signal,  see  Fig.  C.2.  This  time  reference  allows  the  Micro-modem  to  measure  the 
TOA  within  125  ps  corresponding  to  18.75  cm  at  1500  m/s  sound  speed.  The  topside  node  uses  a 
GPS  timeserver  with  timing  accuracy  better  than  10  ps. 

Several  communication  frameworks  exist  for  multiple  underwater  vehicles  operations.  Stokey 
et  al.  (2005)  define  the  widely  used  compact  control  language  (CCL)  message  set,  which  uses  fixed 
bit-fields  to  encode  messages.  Schneider  and  Schmidt  (2010a)  provides  improved  marshalling 
capabilities  with  DCCL,  which  allows  for  dynamically  configurable  messages.  Schneider  and 
Schmidt  (2010b,  2012)  wrap  the  marshalling  capabilities  of  DCCL  with  a  MAC  library  and  lower 
level  modem  drivers.  Webster  et  al.  (2009a)  provide  a  framework  specifically  for  OWTT  navi¬ 
gation.  Finally,  the  CAPTURE  framework  couples  the  low-level  modem  control  with  a  routing 
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Table  C.2  WHOI  Micro-modem  data  packets 


Packet 

Burst  rate 

Frames/packet 

Frame  size 

Packet  length 

PSKR6 

490  bps 

6 

32  B 

2.95  s 

PSKR5 

5388  bps 

8 

256  B 

3.04  s 

PSKR4 

1301  bps 

2 

256  B 

3.15  s 

PSKR3 

1223  bps 

2 

256  B 

3.35  s 

PSKR2 

520  bps 

3 

64  B 

2.95  s 

PSKR1 

498  bps 

3 

64  B 

3.08  s 

FSK  R0 

80  bps 

1 

32  B 

3.90  s 

scheme  (Murphy,  2012;  Murphy  et  al.,  2014). 

All  acoustic  modem  driver-level  and  MAC  scheduling  and  data  marshalling  is  handled  by  the 
goby-acomms  suite  (Schneider  and  Schmidt,  2012)  using  DCCL  messages  with  a  custom  in¬ 
terface  to  the  peris  operating  system.  A  simple  fixed  TDMA  schedule  was  employed.  Each 
vehicle  was  scheduled  to  broadcast  a  data  packet  (either  frequency- shift  keying  (FSK)  or  PSK,  see 
Table  C.2),  a  mini  packet  (13  bit  data  packet),  or  query  the  LBL  network. 

The  acoustic  message  specification  for  the  OSM  (Chapter  2)  is  detailed  in  Table  C.3.  Note  that 
server  state  is  parameterized  by  its  XY  position,  so  that  each  origin  state  packet  information  vector 
and  matrix  are  of  dimension  four  (marginal  distribution  over  origin  and  newest  TOL  states).  In 
pracice,  the  server  vehicle  broadcasts  additional  origin  state  packets,  so  that  each  transmit  event 
includes  at  least  two  Micro-modem  64  B  data  frames. 


Table  C.3  OSM  acoustic  message  specification 


Message  Field 

Range 

Precision 

Maximum  Size 

Depth 

0  —  300  m 

1 

12  bits 

Origin  state  packet: 
Information  vector  element 

I 

o 

1 

o 

rf*. 

5 

31  bits 

Information  matrix  element 

-102  -  102 

7 

31  bits 

Origin  TOF  index 

o 

1 

o 

CO 

- 

10  bits 

New  TOF  index 

O 

1 

o 

CO 

- 

10  bits 

Total 

60  B 
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Table  C.4  Peer-to-peer  acoustic  message  specification 


Message  Field 

Min  value 

Max  value 

Precision 

Maximum  Size 

Depth 

0  m 

—  100  m 

1 

10  bits 

Per  factor  (2  factors  per  link): 
TOL  index 

0 

103 

10  bits 

Measurement  vector  element 

CO 

O 

T— 1 

1 

103 

1 

15  bits 

Covariance  element 

0 

500 

1 

13  bits 

Cross  covariance  element 

-500 

500 

1 

14  bits 

Total  (3  links) 

64  B 

The  acoustic  message  specification  for  broadcasting  links  for  peer-to-peer  navigation  (Chapter  3) 
is  reviewed  in  Table  C.4.  A  link  is  a  two-tuple  representing  two  factors:  the  odometry  factor  to 
the  previous  link  and  a  prior  factor.  Each  factor  has  equal  dimension.  We  were  able  to  specify  the 
range  and  resolution  of  each  factor  so  that  three  links  could  be  broadcast  within  a  single  64  B  data 
frame. 
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